View Javadoc
1   /*
2    * Copyright (C) 2020 Alberto Irurueta Carro (alberto@irurueta.com)
3    *
4    * Licensed under the Apache License, Version 2.0 (the "License");
5    * you may not use this file except in compliance with the License.
6    * You may obtain a copy of the License at
7    *
8    *         http://www.apache.org/licenses/LICENSE-2.0
9    *
10   * Unless required by applicable law or agreed to in writing, software
11   * distributed under the License is distributed on an "AS IS" BASIS,
12   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13   * See the License for the specific language governing permissions and
14   * limitations under the License.
15   */
16  package com.irurueta.navigation.inertial.calibration.accelerometer;
17  
18  import com.irurueta.algebra.Matrix;
19  import com.irurueta.algebra.WrongSizeException;
20  import com.irurueta.navigation.LockedException;
21  import com.irurueta.navigation.NotReadyException;
22  import com.irurueta.navigation.frames.CoordinateTransformation;
23  import com.irurueta.navigation.frames.ECEFFrame;
24  import com.irurueta.navigation.frames.FrameType;
25  import com.irurueta.navigation.frames.InvalidSourceAndDestinationFrameTypeException;
26  import com.irurueta.navigation.frames.NEDFrame;
27  import com.irurueta.navigation.frames.converters.NEDtoECEFFrameConverter;
28  import com.irurueta.navigation.frames.converters.NEDtoECEFPositionVelocityConverter;
29  import com.irurueta.navigation.geodesic.Constants;
30  import com.irurueta.navigation.inertial.BodyKinematics;
31  import com.irurueta.navigation.inertial.ECEFGravity;
32  import com.irurueta.navigation.inertial.ECEFPosition;
33  import com.irurueta.navigation.inertial.ECEFVelocity;
34  import com.irurueta.navigation.inertial.NEDPosition;
35  import com.irurueta.navigation.inertial.NEDVelocity;
36  import com.irurueta.navigation.inertial.calibration.BodyKinematicsGenerator;
37  import com.irurueta.navigation.inertial.calibration.CalibrationException;
38  import com.irurueta.navigation.inertial.calibration.IMUErrors;
39  import com.irurueta.navigation.inertial.calibration.StandardDeviationBodyKinematics;
40  import com.irurueta.navigation.inertial.estimators.ECEFGravityEstimator;
41  import com.irurueta.navigation.inertial.estimators.ECEFKinematicsEstimator;
42  import com.irurueta.statistics.UniformRandomizer;
43  import com.irurueta.units.Acceleration;
44  import com.irurueta.units.AccelerationUnit;
45  import org.junit.Test;
46  
47  import java.util.ArrayList;
48  import java.util.Collection;
49  import java.util.Collections;
50  import java.util.List;
51  import java.util.Random;
52  
53  import static org.junit.Assert.*;
54  
55  public class KnownPositionAccelerometerCalibratorTest implements
56          KnownPositionAccelerometerCalibratorListener {
57  
58      private static final double TIME_INTERVAL_SECONDS = 0.02;
59  
60      private static final double MICRO_G_TO_METERS_PER_SECOND_SQUARED = 9.80665E-6;
61      private static final double DEG_TO_RAD = 0.01745329252;
62  
63      private static final double MIN_ANGLE_DEGREES = -180.0;
64      private static final double MAX_ANGLE_DEGREES = 180.0;
65  
66      private static final double MIN_LATITUDE_DEGREES = -90.0;
67      private static final double MAX_LATITUDE_DEGREEs = 90.0;
68      private static final double MIN_LONGITUDE_DEGREES = -180.0;
69      private static final double MAX_LONGITUDE_DEGREES = 180.0;
70      private static final double MIN_HEIGHT = -50.0;
71      private static final double MAX_HEIGHT = 50.0;
72  
73      private static final int LARGE_MEASUREMENT_NUMBER = 100000;
74  
75      private static final double ABSOLUTE_ERROR = 1e-8;
76      private static final double LARGE_ABSOLUTE_ERROR = 5e-5;
77      private static final double VERY_LARGE_ABSOLUTE_ERROR = 1e-3;
78  
79      private static final int TIMES = 100;
80  
81      private int mCalibrateStart;
82      private int mCalibrateEnd;
83  
84      @Test
85      public void testConstructor1() throws WrongSizeException {
86          final KnownPositionAccelerometerCalibrator calibrator =
87                  new KnownPositionAccelerometerCalibrator();
88  
89          // check default values
90          assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
91          assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
92          assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
93          final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
94          assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
95          assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
96          final Acceleration bx2 = new Acceleration(0.0,
97                  AccelerationUnit.FEET_PER_SQUARED_SECOND);
98          calibrator.getInitialBiasXAsAcceleration(bx2);
99          assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
100         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
101         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
102         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
103         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
104         final Acceleration by2 = new Acceleration(0.0,
105                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
106         calibrator.getInitialBiasYAsAcceleration(by2);
107         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
108         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
109         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
110         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
111         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
112         final Acceleration bz2 = new Acceleration(0.0,
113                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
114         calibrator.getInitialBiasZAsAcceleration(bz2);
115         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
116         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
117         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
118         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
119         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
120         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
121         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
122         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
123         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
124         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
125         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
126         final double[] bias1 = calibrator.getInitialBias();
127         assertArrayEquals(bias1, new double[3], 0.0);
128         final double[] bias2 = new double[3];
129         calibrator.getInitialBias(bias2);
130         assertArrayEquals(bias1, bias2, 0.0);
131         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
132         assertEquals(b1, new Matrix(3, 1));
133         final Matrix b2 = new Matrix(3, 1);
134         calibrator.getInitialBiasAsMatrix(b2);
135         assertEquals(b1, b2);
136         final Matrix ma1 = calibrator.getInitialMa();
137         assertEquals(ma1, new Matrix(3, 3));
138         final Matrix ma2 = new Matrix(3, 3);
139         calibrator.getInitialMa(ma2);
140         assertEquals(ma1, ma2);
141         assertNull(calibrator.getEcefPosition());
142         assertNull(calibrator.getNedPosition());
143         assertFalse(calibrator.getNedPosition(null));
144         assertNull(calibrator.getMeasurements());
145         assertFalse(calibrator.isCommonAxisUsed());
146         assertNull(calibrator.getListener());
147         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
148         assertFalse(calibrator.isReady());
149         assertFalse(calibrator.isRunning());
150         assertNull(calibrator.getEstimatedBiases());
151         assertFalse(calibrator.getEstimatedBiases(null));
152         assertNull(calibrator.getEstimatedBiasesAsMatrix());
153         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
154         assertNull(calibrator.getEstimatedBiasFx());
155         assertNull(calibrator.getEstimatedBiasFy());
156         assertNull(calibrator.getEstimatedBiasFz());
157         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
158         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
159         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
160         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
161         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
162         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
163         assertNull(calibrator.getEstimatedMa());
164         assertNull(calibrator.getEstimatedSx());
165         assertNull(calibrator.getEstimatedSy());
166         assertNull(calibrator.getEstimatedSz());
167         assertNull(calibrator.getEstimatedMxy());
168         assertNull(calibrator.getEstimatedMxz());
169         assertNull(calibrator.getEstimatedMyx());
170         assertNull(calibrator.getEstimatedMyz());
171         assertNull(calibrator.getEstimatedMzx());
172         assertNull(calibrator.getEstimatedMzy());
173         assertNull(calibrator.getEstimatedCovariance());
174         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
175         assertNull(calibrator.getGroundTruthGravityNorm());
176         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
177         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
178     }
179 
180     @Test
181     public void testConstructor2() throws WrongSizeException {
182         final KnownPositionAccelerometerCalibrator calibrator =
183                 new KnownPositionAccelerometerCalibrator(this);
184 
185         // check default values
186         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
187         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
188         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
189         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
190         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
191         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
192         final Acceleration bx2 = new Acceleration(0.0,
193                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
194         calibrator.getInitialBiasXAsAcceleration(bx2);
195         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
196         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
197         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
198         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
199         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
200         final Acceleration by2 = new Acceleration(0.0,
201                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
202         calibrator.getInitialBiasYAsAcceleration(by2);
203         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
204         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
205         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
206         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
207         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
208         final Acceleration bz2 = new Acceleration(0.0,
209                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
210         calibrator.getInitialBiasZAsAcceleration(bz2);
211         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
212         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
213         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
214         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
215         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
216         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
217         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
218         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
219         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
220         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
221         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
222         final double[] bias1 = calibrator.getInitialBias();
223         assertArrayEquals(bias1, new double[3], 0.0);
224         final double[] bias2 = new double[3];
225         calibrator.getInitialBias(bias2);
226         assertArrayEquals(bias1, bias2, 0.0);
227         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
228         assertEquals(b1, new Matrix(3, 1));
229         final Matrix b2 = new Matrix(3, 1);
230         calibrator.getInitialBiasAsMatrix(b2);
231         assertEquals(b1, b2);
232         final Matrix ma1 = calibrator.getInitialMa();
233         assertEquals(ma1, new Matrix(3, 3));
234         final Matrix ma2 = new Matrix(3, 3);
235         calibrator.getInitialMa(ma2);
236         assertEquals(ma1, ma2);
237         assertNull(calibrator.getEcefPosition());
238         assertNull(calibrator.getNedPosition());
239         assertFalse(calibrator.getNedPosition(null));
240         assertNull(calibrator.getMeasurements());
241         assertFalse(calibrator.isCommonAxisUsed());
242         assertSame(calibrator.getListener(), this);
243         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
244         assertFalse(calibrator.isReady());
245         assertFalse(calibrator.isRunning());
246         assertNull(calibrator.getEstimatedBiases());
247         assertFalse(calibrator.getEstimatedBiases(null));
248         assertNull(calibrator.getEstimatedBiasesAsMatrix());
249         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
250         assertNull(calibrator.getEstimatedBiasFx());
251         assertNull(calibrator.getEstimatedBiasFy());
252         assertNull(calibrator.getEstimatedBiasFz());
253         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
254         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
255         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
256         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
257         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
258         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
259         assertNull(calibrator.getEstimatedMa());
260         assertNull(calibrator.getEstimatedSx());
261         assertNull(calibrator.getEstimatedSy());
262         assertNull(calibrator.getEstimatedSz());
263         assertNull(calibrator.getEstimatedMxy());
264         assertNull(calibrator.getEstimatedMxz());
265         assertNull(calibrator.getEstimatedMyx());
266         assertNull(calibrator.getEstimatedMyz());
267         assertNull(calibrator.getEstimatedMzx());
268         assertNull(calibrator.getEstimatedMzy());
269         assertNull(calibrator.getEstimatedCovariance());
270         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
271         assertNull(calibrator.getGroundTruthGravityNorm());
272         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
273         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
274     }
275 
276     @Test
277     public void testConstructor3() throws WrongSizeException {
278         final Collection<StandardDeviationBodyKinematics> measurements =
279                 Collections.emptyList();
280         final KnownPositionAccelerometerCalibrator calibrator =
281                 new KnownPositionAccelerometerCalibrator(measurements);
282 
283         // check default values
284         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
285         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
286         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
287         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
288         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
289         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
290         final Acceleration bx2 = new Acceleration(0.0,
291                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
292         calibrator.getInitialBiasXAsAcceleration(bx2);
293         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
294         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
295         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
296         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
297         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
298         final Acceleration by2 = new Acceleration(0.0,
299                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
300         calibrator.getInitialBiasYAsAcceleration(by2);
301         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
302         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
303         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
304         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
305         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
306         final Acceleration bz2 = new Acceleration(0.0,
307                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
308         calibrator.getInitialBiasZAsAcceleration(bz2);
309         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
310         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
311         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
312         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
313         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
314         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
315         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
316         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
317         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
318         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
319         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
320         final double[] bias1 = calibrator.getInitialBias();
321         assertArrayEquals(bias1, new double[3], 0.0);
322         final double[] bias2 = new double[3];
323         calibrator.getInitialBias(bias2);
324         assertArrayEquals(bias1, bias2, 0.0);
325         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
326         assertEquals(b1, new Matrix(3, 1));
327         final Matrix b2 = new Matrix(3, 1);
328         calibrator.getInitialBiasAsMatrix(b2);
329         assertEquals(b1, b2);
330         final Matrix ma1 = calibrator.getInitialMa();
331         assertEquals(ma1, new Matrix(3, 3));
332         final Matrix ma2 = new Matrix(3, 3);
333         calibrator.getInitialMa(ma2);
334         assertEquals(ma1, ma2);
335         assertNull(calibrator.getEcefPosition());
336         assertNull(calibrator.getNedPosition());
337         assertFalse(calibrator.getNedPosition(null));
338         assertSame(calibrator.getMeasurements(), measurements);
339         assertFalse(calibrator.isCommonAxisUsed());
340         assertNull(calibrator.getListener());
341         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
342         assertFalse(calibrator.isReady());
343         assertFalse(calibrator.isRunning());
344         assertNull(calibrator.getEstimatedBiases());
345         assertFalse(calibrator.getEstimatedBiases(null));
346         assertNull(calibrator.getEstimatedBiasesAsMatrix());
347         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
348         assertNull(calibrator.getEstimatedBiasFx());
349         assertNull(calibrator.getEstimatedBiasFy());
350         assertNull(calibrator.getEstimatedBiasFz());
351         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
352         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
353         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
354         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
355         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
356         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
357         assertNull(calibrator.getEstimatedMa());
358         assertNull(calibrator.getEstimatedSx());
359         assertNull(calibrator.getEstimatedSy());
360         assertNull(calibrator.getEstimatedSz());
361         assertNull(calibrator.getEstimatedMxy());
362         assertNull(calibrator.getEstimatedMxz());
363         assertNull(calibrator.getEstimatedMyx());
364         assertNull(calibrator.getEstimatedMyz());
365         assertNull(calibrator.getEstimatedMzx());
366         assertNull(calibrator.getEstimatedMzy());
367         assertNull(calibrator.getEstimatedCovariance());
368         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
369         assertNull(calibrator.getGroundTruthGravityNorm());
370         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
371         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
372     }
373 
374     @Test
375     public void testConstructor4() throws WrongSizeException {
376         final Collection<StandardDeviationBodyKinematics> measurements =
377                 Collections.emptyList();
378         final KnownPositionAccelerometerCalibrator calibrator =
379                 new KnownPositionAccelerometerCalibrator(measurements, this);
380 
381         // check default values
382         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
383         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
384         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
385         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
386         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
387         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
388         final Acceleration bx2 = new Acceleration(0.0,
389                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
390         calibrator.getInitialBiasXAsAcceleration(bx2);
391         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
392         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
393         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
394         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
395         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
396         final Acceleration by2 = new Acceleration(0.0,
397                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
398         calibrator.getInitialBiasYAsAcceleration(by2);
399         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
400         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
401         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
402         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
403         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
404         final Acceleration bz2 = new Acceleration(0.0,
405                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
406         calibrator.getInitialBiasZAsAcceleration(bz2);
407         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
408         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
409         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
410         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
411         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
412         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
413         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
414         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
415         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
416         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
417         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
418         final double[] bias1 = calibrator.getInitialBias();
419         assertArrayEquals(bias1, new double[3], 0.0);
420         final double[] bias2 = new double[3];
421         calibrator.getInitialBias(bias2);
422         assertArrayEquals(bias1, bias2, 0.0);
423         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
424         assertEquals(b1, new Matrix(3, 1));
425         final Matrix b2 = new Matrix(3, 1);
426         calibrator.getInitialBiasAsMatrix(b2);
427         assertEquals(b1, b2);
428         final Matrix ma1 = calibrator.getInitialMa();
429         assertEquals(ma1, new Matrix(3, 3));
430         final Matrix ma2 = new Matrix(3, 3);
431         calibrator.getInitialMa(ma2);
432         assertEquals(ma1, ma2);
433         assertNull(calibrator.getEcefPosition());
434         assertNull(calibrator.getNedPosition());
435         assertFalse(calibrator.getNedPosition(null));
436         assertSame(calibrator.getMeasurements(), measurements);
437         assertFalse(calibrator.isCommonAxisUsed());
438         assertSame(calibrator.getListener(), this);
439         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
440         assertFalse(calibrator.isReady());
441         assertFalse(calibrator.isRunning());
442         assertNull(calibrator.getEstimatedBiases());
443         assertFalse(calibrator.getEstimatedBiases(null));
444         assertNull(calibrator.getEstimatedBiasesAsMatrix());
445         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
446         assertNull(calibrator.getEstimatedBiasFx());
447         assertNull(calibrator.getEstimatedBiasFy());
448         assertNull(calibrator.getEstimatedBiasFz());
449         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
450         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
451         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
452         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
453         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
454         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
455         assertNull(calibrator.getEstimatedMa());
456         assertNull(calibrator.getEstimatedSx());
457         assertNull(calibrator.getEstimatedSy());
458         assertNull(calibrator.getEstimatedSz());
459         assertNull(calibrator.getEstimatedMxy());
460         assertNull(calibrator.getEstimatedMxz());
461         assertNull(calibrator.getEstimatedMyx());
462         assertNull(calibrator.getEstimatedMyz());
463         assertNull(calibrator.getEstimatedMzx());
464         assertNull(calibrator.getEstimatedMzy());
465         assertNull(calibrator.getEstimatedCovariance());
466         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
467         assertNull(calibrator.getGroundTruthGravityNorm());
468         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
469         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
470     }
471 
472     @Test
473     public void testConstructor5() throws WrongSizeException {
474         final KnownPositionAccelerometerCalibrator calibrator =
475                 new KnownPositionAccelerometerCalibrator(true);
476 
477         // check default values
478         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
479         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
480         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
481         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
482         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
483         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
484         final Acceleration bx2 = new Acceleration(0.0,
485                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
486         calibrator.getInitialBiasXAsAcceleration(bx2);
487         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
488         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
489         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
490         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
491         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
492         final Acceleration by2 = new Acceleration(0.0,
493                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
494         calibrator.getInitialBiasYAsAcceleration(by2);
495         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
496         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
497         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
498         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
499         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
500         final Acceleration bz2 = new Acceleration(0.0,
501                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
502         calibrator.getInitialBiasZAsAcceleration(bz2);
503         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
504         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
505         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
506         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
507         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
508         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
509         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
510         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
511         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
512         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
513         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
514         final double[] bias1 = calibrator.getInitialBias();
515         assertArrayEquals(bias1, new double[3], 0.0);
516         final double[] bias2 = new double[3];
517         calibrator.getInitialBias(bias2);
518         assertArrayEquals(bias1, bias2, 0.0);
519         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
520         assertEquals(b1, new Matrix(3, 1));
521         final Matrix b2 = new Matrix(3, 1);
522         calibrator.getInitialBiasAsMatrix(b2);
523         assertEquals(b1, b2);
524         final Matrix ma1 = calibrator.getInitialMa();
525         assertEquals(ma1, new Matrix(3, 3));
526         final Matrix ma2 = new Matrix(3, 3);
527         calibrator.getInitialMa(ma2);
528         assertEquals(ma1, ma2);
529         assertNull(calibrator.getEcefPosition());
530         assertNull(calibrator.getNedPosition());
531         assertFalse(calibrator.getNedPosition(null));
532         assertNull(calibrator.getMeasurements());
533         assertTrue(calibrator.isCommonAxisUsed());
534         assertNull(calibrator.getListener());
535         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
536         assertFalse(calibrator.isReady());
537         assertFalse(calibrator.isRunning());
538         assertNull(calibrator.getEstimatedBiases());
539         assertFalse(calibrator.getEstimatedBiases(null));
540         assertNull(calibrator.getEstimatedBiasesAsMatrix());
541         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
542         assertNull(calibrator.getEstimatedBiasFx());
543         assertNull(calibrator.getEstimatedBiasFy());
544         assertNull(calibrator.getEstimatedBiasFz());
545         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
546         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
547         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
548         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
549         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
550         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
551         assertNull(calibrator.getEstimatedMa());
552         assertNull(calibrator.getEstimatedSx());
553         assertNull(calibrator.getEstimatedSy());
554         assertNull(calibrator.getEstimatedSz());
555         assertNull(calibrator.getEstimatedMxy());
556         assertNull(calibrator.getEstimatedMxz());
557         assertNull(calibrator.getEstimatedMyx());
558         assertNull(calibrator.getEstimatedMyz());
559         assertNull(calibrator.getEstimatedMzx());
560         assertNull(calibrator.getEstimatedMzy());
561         assertNull(calibrator.getEstimatedCovariance());
562         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
563         assertNull(calibrator.getGroundTruthGravityNorm());
564         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
565         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
566     }
567 
568     @Test
569     public void testConstructor6() throws WrongSizeException {
570         final KnownPositionAccelerometerCalibrator calibrator =
571                 new KnownPositionAccelerometerCalibrator(true,
572                         this);
573 
574         // check default values
575         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
576         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
577         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
578         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
579         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
580         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
581         final Acceleration bx2 = new Acceleration(0.0,
582                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
583         calibrator.getInitialBiasXAsAcceleration(bx2);
584         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
585         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
586         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
587         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
588         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
589         final Acceleration by2 = new Acceleration(0.0,
590                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
591         calibrator.getInitialBiasYAsAcceleration(by2);
592         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
593         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
594         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
595         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
596         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
597         final Acceleration bz2 = new Acceleration(0.0,
598                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
599         calibrator.getInitialBiasZAsAcceleration(bz2);
600         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
601         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
602         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
603         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
604         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
605         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
606         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
607         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
608         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
609         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
610         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
611         final double[] bias1 = calibrator.getInitialBias();
612         assertArrayEquals(bias1, new double[3], 0.0);
613         final double[] bias2 = new double[3];
614         calibrator.getInitialBias(bias2);
615         assertArrayEquals(bias1, bias2, 0.0);
616         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
617         assertEquals(b1, new Matrix(3, 1));
618         final Matrix b2 = new Matrix(3, 1);
619         calibrator.getInitialBiasAsMatrix(b2);
620         assertEquals(b1, b2);
621         final Matrix ma1 = calibrator.getInitialMa();
622         assertEquals(ma1, new Matrix(3, 3));
623         final Matrix ma2 = new Matrix(3, 3);
624         calibrator.getInitialMa(ma2);
625         assertEquals(ma1, ma2);
626         assertNull(calibrator.getEcefPosition());
627         assertNull(calibrator.getNedPosition());
628         assertFalse(calibrator.getNedPosition(null));
629         assertNull(calibrator.getMeasurements());
630         assertTrue(calibrator.isCommonAxisUsed());
631         assertSame(calibrator.getListener(), this);
632         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
633         assertFalse(calibrator.isReady());
634         assertFalse(calibrator.isRunning());
635         assertNull(calibrator.getEstimatedBiases());
636         assertFalse(calibrator.getEstimatedBiases(null));
637         assertNull(calibrator.getEstimatedBiasesAsMatrix());
638         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
639         assertNull(calibrator.getEstimatedBiasFx());
640         assertNull(calibrator.getEstimatedBiasFy());
641         assertNull(calibrator.getEstimatedBiasFz());
642         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
643         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
644         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
645         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
646         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
647         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
648         assertNull(calibrator.getEstimatedMa());
649         assertNull(calibrator.getEstimatedSx());
650         assertNull(calibrator.getEstimatedSy());
651         assertNull(calibrator.getEstimatedSz());
652         assertNull(calibrator.getEstimatedMxy());
653         assertNull(calibrator.getEstimatedMxz());
654         assertNull(calibrator.getEstimatedMyx());
655         assertNull(calibrator.getEstimatedMyz());
656         assertNull(calibrator.getEstimatedMzx());
657         assertNull(calibrator.getEstimatedMzy());
658         assertNull(calibrator.getEstimatedCovariance());
659         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
660         assertNull(calibrator.getGroundTruthGravityNorm());
661         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
662         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
663     }
664 
665     @Test
666     public void testConstructor7() throws WrongSizeException {
667         final Collection<StandardDeviationBodyKinematics> measurements =
668                 Collections.emptyList();
669         final KnownPositionAccelerometerCalibrator calibrator =
670                 new KnownPositionAccelerometerCalibrator(measurements,
671                         true);
672 
673         // check default values
674         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
675         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
676         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
677         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
678         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
679         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
680         final Acceleration bx2 = new Acceleration(0.0,
681                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
682         calibrator.getInitialBiasXAsAcceleration(bx2);
683         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
684         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
685         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
686         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
687         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
688         final Acceleration by2 = new Acceleration(0.0,
689                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
690         calibrator.getInitialBiasYAsAcceleration(by2);
691         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
692         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
693         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
694         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
695         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
696         final Acceleration bz2 = new Acceleration(0.0,
697                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
698         calibrator.getInitialBiasZAsAcceleration(bz2);
699         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
700         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
701         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
702         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
703         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
704         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
705         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
706         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
707         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
708         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
709         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
710         final double[] bias1 = calibrator.getInitialBias();
711         assertArrayEquals(bias1, new double[3], 0.0);
712         final double[] bias2 = new double[3];
713         calibrator.getInitialBias(bias2);
714         assertArrayEquals(bias1, bias2, 0.0);
715         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
716         assertEquals(b1, new Matrix(3, 1));
717         final Matrix b2 = new Matrix(3, 1);
718         calibrator.getInitialBiasAsMatrix(b2);
719         assertEquals(b1, b2);
720         final Matrix ma1 = calibrator.getInitialMa();
721         assertEquals(ma1, new Matrix(3, 3));
722         final Matrix ma2 = new Matrix(3, 3);
723         calibrator.getInitialMa(ma2);
724         assertEquals(ma1, ma2);
725         assertNull(calibrator.getEcefPosition());
726         assertNull(calibrator.getNedPosition());
727         assertFalse(calibrator.getNedPosition(null));
728         assertSame(calibrator.getMeasurements(), measurements);
729         assertTrue(calibrator.isCommonAxisUsed());
730         assertNull(calibrator.getListener());
731         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
732         assertFalse(calibrator.isReady());
733         assertFalse(calibrator.isRunning());
734         assertNull(calibrator.getEstimatedBiases());
735         assertFalse(calibrator.getEstimatedBiases(null));
736         assertNull(calibrator.getEstimatedBiasesAsMatrix());
737         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
738         assertNull(calibrator.getEstimatedBiasFx());
739         assertNull(calibrator.getEstimatedBiasFy());
740         assertNull(calibrator.getEstimatedBiasFz());
741         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
742         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
743         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
744         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
745         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
746         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
747         assertNull(calibrator.getEstimatedMa());
748         assertNull(calibrator.getEstimatedSx());
749         assertNull(calibrator.getEstimatedSy());
750         assertNull(calibrator.getEstimatedSz());
751         assertNull(calibrator.getEstimatedMxy());
752         assertNull(calibrator.getEstimatedMxz());
753         assertNull(calibrator.getEstimatedMyx());
754         assertNull(calibrator.getEstimatedMyz());
755         assertNull(calibrator.getEstimatedMzx());
756         assertNull(calibrator.getEstimatedMzy());
757         assertNull(calibrator.getEstimatedCovariance());
758         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
759         assertNull(calibrator.getGroundTruthGravityNorm());
760         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
761         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
762     }
763 
764     @Test
765     public void testConstructor8() throws WrongSizeException {
766         final Collection<StandardDeviationBodyKinematics> measurements =
767                 Collections.emptyList();
768         final KnownPositionAccelerometerCalibrator calibrator =
769                 new KnownPositionAccelerometerCalibrator(measurements,
770                         true, this);
771 
772         // check default values
773         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
774         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
775         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
776         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
777         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
778         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
779         final Acceleration bx2 = new Acceleration(0.0,
780                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
781         calibrator.getInitialBiasXAsAcceleration(bx2);
782         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
783         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
784         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
785         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
786         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
787         final Acceleration by2 = new Acceleration(0.0,
788                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
789         calibrator.getInitialBiasYAsAcceleration(by2);
790         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
791         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
792         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
793         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
794         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
795         final Acceleration bz2 = new Acceleration(0.0,
796                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
797         calibrator.getInitialBiasZAsAcceleration(bz2);
798         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
799         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
800         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
801         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
802         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
803         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
804         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
805         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
806         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
807         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
808         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
809         final double[] bias1 = calibrator.getInitialBias();
810         assertArrayEquals(bias1, new double[3], 0.0);
811         final double[] bias2 = new double[3];
812         calibrator.getInitialBias(bias2);
813         assertArrayEquals(bias1, bias2, 0.0);
814         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
815         assertEquals(b1, new Matrix(3, 1));
816         final Matrix b2 = new Matrix(3, 1);
817         calibrator.getInitialBiasAsMatrix(b2);
818         assertEquals(b1, b2);
819         final Matrix ma1 = calibrator.getInitialMa();
820         assertEquals(ma1, new Matrix(3, 3));
821         final Matrix ma2 = new Matrix(3, 3);
822         calibrator.getInitialMa(ma2);
823         assertEquals(ma1, ma2);
824         assertNull(calibrator.getEcefPosition());
825         assertNull(calibrator.getNedPosition());
826         assertFalse(calibrator.getNedPosition(null));
827         assertSame(calibrator.getMeasurements(), measurements);
828         assertTrue(calibrator.isCommonAxisUsed());
829         assertSame(calibrator.getListener(), this);
830         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
831         assertFalse(calibrator.isReady());
832         assertFalse(calibrator.isRunning());
833         assertNull(calibrator.getEstimatedBiases());
834         assertFalse(calibrator.getEstimatedBiases(null));
835         assertNull(calibrator.getEstimatedBiasesAsMatrix());
836         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
837         assertNull(calibrator.getEstimatedBiasFx());
838         assertNull(calibrator.getEstimatedBiasFy());
839         assertNull(calibrator.getEstimatedBiasFz());
840         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
841         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
842         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
843         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
844         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
845         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
846         assertNull(calibrator.getEstimatedMa());
847         assertNull(calibrator.getEstimatedSx());
848         assertNull(calibrator.getEstimatedSy());
849         assertNull(calibrator.getEstimatedSz());
850         assertNull(calibrator.getEstimatedMxy());
851         assertNull(calibrator.getEstimatedMxz());
852         assertNull(calibrator.getEstimatedMyx());
853         assertNull(calibrator.getEstimatedMyz());
854         assertNull(calibrator.getEstimatedMzx());
855         assertNull(calibrator.getEstimatedMzy());
856         assertNull(calibrator.getEstimatedCovariance());
857         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
858         assertNull(calibrator.getGroundTruthGravityNorm());
859         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
860         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
861     }
862 
863     @Test
864     public void testConstructor9() throws WrongSizeException {
865         final Matrix ba = generateBa();
866         final double biasX = ba.getElementAtIndex(0);
867         final double biasY = ba.getElementAtIndex(1);
868         final double biasZ = ba.getElementAtIndex(2);
869 
870         final KnownPositionAccelerometerCalibrator calibrator =
871                 new KnownPositionAccelerometerCalibrator(biasX, biasY, biasZ);
872 
873         // check default values
874         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
875         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
876         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
877         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
878         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
879         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
880         final Acceleration bx2 = new Acceleration(0.0,
881                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
882         calibrator.getInitialBiasXAsAcceleration(bx2);
883         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
884         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
885         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
886         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
887         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
888         final Acceleration by2 = new Acceleration(0.0,
889                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
890         calibrator.getInitialBiasYAsAcceleration(by2);
891         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
892         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
893         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
894         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
895         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
896         final Acceleration bz2 = new Acceleration(0.0,
897                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
898         calibrator.getInitialBiasZAsAcceleration(bz2);
899         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
900         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
901         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
902         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
903         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
904         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
905         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
906         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
907         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
908         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
909         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
910         final double[] bias1 = calibrator.getInitialBias();
911         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
912         final double[] bias2 = new double[3];
913         calibrator.getInitialBias(bias2);
914         assertArrayEquals(bias1, bias2, 0.0);
915         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
916         assertEquals(b1, ba);
917         final Matrix b2 = new Matrix(3, 1);
918         calibrator.getInitialBiasAsMatrix(b2);
919         assertEquals(b1, b2);
920         final Matrix ma1 = calibrator.getInitialMa();
921         assertEquals(ma1, new Matrix(3, 3));
922         final Matrix ma2 = new Matrix(3, 3);
923         calibrator.getInitialMa(ma2);
924         assertEquals(ma1, ma2);
925         assertNull(calibrator.getEcefPosition());
926         assertNull(calibrator.getNedPosition());
927         assertFalse(calibrator.getNedPosition(null));
928         assertNull(calibrator.getMeasurements());
929         assertFalse(calibrator.isCommonAxisUsed());
930         assertNull(calibrator.getListener());
931         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
932         assertFalse(calibrator.isReady());
933         assertFalse(calibrator.isRunning());
934         assertNull(calibrator.getEstimatedBiases());
935         assertFalse(calibrator.getEstimatedBiases(null));
936         assertNull(calibrator.getEstimatedBiasesAsMatrix());
937         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
938         assertNull(calibrator.getEstimatedBiasFx());
939         assertNull(calibrator.getEstimatedBiasFy());
940         assertNull(calibrator.getEstimatedBiasFz());
941         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
942         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
943         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
944         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
945         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
946         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
947         assertNull(calibrator.getEstimatedMa());
948         assertNull(calibrator.getEstimatedSx());
949         assertNull(calibrator.getEstimatedSy());
950         assertNull(calibrator.getEstimatedSz());
951         assertNull(calibrator.getEstimatedMxy());
952         assertNull(calibrator.getEstimatedMxz());
953         assertNull(calibrator.getEstimatedMyx());
954         assertNull(calibrator.getEstimatedMyz());
955         assertNull(calibrator.getEstimatedMzx());
956         assertNull(calibrator.getEstimatedMzy());
957         assertNull(calibrator.getEstimatedCovariance());
958         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
959         assertNull(calibrator.getGroundTruthGravityNorm());
960         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
961         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
962     }
963 
964     @Test
965     public void testConstructor10() throws WrongSizeException {
966         final Matrix ba = generateBa();
967         final double biasX = ba.getElementAtIndex(0);
968         final double biasY = ba.getElementAtIndex(1);
969         final double biasZ = ba.getElementAtIndex(2);
970 
971         final KnownPositionAccelerometerCalibrator calibrator =
972                 new KnownPositionAccelerometerCalibrator(biasX, biasY, biasZ,
973                         this);
974 
975         // check default values
976         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
977         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
978         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
979         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
980         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
981         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
982         final Acceleration bx2 = new Acceleration(0.0,
983                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
984         calibrator.getInitialBiasXAsAcceleration(bx2);
985         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
986         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
987         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
988         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
989         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
990         final Acceleration by2 = new Acceleration(0.0,
991                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
992         calibrator.getInitialBiasYAsAcceleration(by2);
993         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
994         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
995         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
996         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
997         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
998         final Acceleration bz2 = new Acceleration(0.0,
999                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1000         calibrator.getInitialBiasZAsAcceleration(bz2);
1001         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1002         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1003         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1004         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1005         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1006         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1007         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1008         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1009         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1010         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1011         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1012         final double[] bias1 = calibrator.getInitialBias();
1013         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1014         final double[] bias2 = new double[3];
1015         calibrator.getInitialBias(bias2);
1016         assertArrayEquals(bias1, bias2, 0.0);
1017         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1018         assertEquals(b1, ba);
1019         final Matrix b2 = new Matrix(3, 1);
1020         calibrator.getInitialBiasAsMatrix(b2);
1021         assertEquals(b1, b2);
1022         final Matrix ma1 = calibrator.getInitialMa();
1023         assertEquals(ma1, new Matrix(3, 3));
1024         final Matrix ma2 = new Matrix(3, 3);
1025         calibrator.getInitialMa(ma2);
1026         assertEquals(ma1, ma2);
1027         assertNull(calibrator.getEcefPosition());
1028         assertNull(calibrator.getNedPosition());
1029         assertFalse(calibrator.getNedPosition(null));
1030         assertNull(calibrator.getMeasurements());
1031         assertFalse(calibrator.isCommonAxisUsed());
1032         assertSame(calibrator.getListener(), this);
1033         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1034         assertFalse(calibrator.isReady());
1035         assertFalse(calibrator.isRunning());
1036         assertNull(calibrator.getEstimatedBiases());
1037         assertFalse(calibrator.getEstimatedBiases(null));
1038         assertNull(calibrator.getEstimatedBiasesAsMatrix());
1039         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1040         assertNull(calibrator.getEstimatedBiasFx());
1041         assertNull(calibrator.getEstimatedBiasFy());
1042         assertNull(calibrator.getEstimatedBiasFz());
1043         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1044         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1045         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1046         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1047         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1048         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1049         assertNull(calibrator.getEstimatedMa());
1050         assertNull(calibrator.getEstimatedSx());
1051         assertNull(calibrator.getEstimatedSy());
1052         assertNull(calibrator.getEstimatedSz());
1053         assertNull(calibrator.getEstimatedMxy());
1054         assertNull(calibrator.getEstimatedMxz());
1055         assertNull(calibrator.getEstimatedMyx());
1056         assertNull(calibrator.getEstimatedMyz());
1057         assertNull(calibrator.getEstimatedMzx());
1058         assertNull(calibrator.getEstimatedMzy());
1059         assertNull(calibrator.getEstimatedCovariance());
1060         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1061         assertNull(calibrator.getGroundTruthGravityNorm());
1062         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1063         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1064     }
1065 
1066     @Test
1067     public void testConstructor11() throws WrongSizeException {
1068         final Collection<StandardDeviationBodyKinematics> measurements =
1069                 Collections.emptyList();
1070 
1071         final Matrix ba = generateBa();
1072         final double biasX = ba.getElementAtIndex(0);
1073         final double biasY = ba.getElementAtIndex(1);
1074         final double biasZ = ba.getElementAtIndex(2);
1075 
1076         final KnownPositionAccelerometerCalibrator calibrator =
1077                 new KnownPositionAccelerometerCalibrator(measurements,
1078                         biasX, biasY, biasZ);
1079 
1080         // check default values
1081         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1082         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1083         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1084         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1085         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1086         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1087         final Acceleration bx2 = new Acceleration(0.0,
1088                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1089         calibrator.getInitialBiasXAsAcceleration(bx2);
1090         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1091         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1092         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1093         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1094         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1095         final Acceleration by2 = new Acceleration(0.0,
1096                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1097         calibrator.getInitialBiasYAsAcceleration(by2);
1098         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1099         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1100         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1101         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1102         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1103         final Acceleration bz2 = new Acceleration(0.0,
1104                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1105         calibrator.getInitialBiasZAsAcceleration(bz2);
1106         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1107         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1108         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1109         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1110         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1111         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1112         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1113         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1114         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1115         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1116         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1117         final double[] bias1 = calibrator.getInitialBias();
1118         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1119         final double[] bias2 = new double[3];
1120         calibrator.getInitialBias(bias2);
1121         assertArrayEquals(bias1, bias2, 0.0);
1122         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1123         assertEquals(b1, ba);
1124         final Matrix b2 = new Matrix(3, 1);
1125         calibrator.getInitialBiasAsMatrix(b2);
1126         assertEquals(b1, b2);
1127         final Matrix ma1 = calibrator.getInitialMa();
1128         assertEquals(ma1, new Matrix(3, 3));
1129         final Matrix ma2 = new Matrix(3, 3);
1130         calibrator.getInitialMa(ma2);
1131         assertEquals(ma1, ma2);
1132         assertNull(calibrator.getEcefPosition());
1133         assertNull(calibrator.getNedPosition());
1134         assertFalse(calibrator.getNedPosition(null));
1135         assertSame(calibrator.getMeasurements(), measurements);
1136         assertFalse(calibrator.isCommonAxisUsed());
1137         assertNull(calibrator.getListener());
1138         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1139         assertFalse(calibrator.isReady());
1140         assertFalse(calibrator.isRunning());
1141         assertNull(calibrator.getEstimatedBiases());
1142         assertFalse(calibrator.getEstimatedBiases(null));
1143         assertNull(calibrator.getEstimatedBiasesAsMatrix());
1144         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1145         assertNull(calibrator.getEstimatedBiasFx());
1146         assertNull(calibrator.getEstimatedBiasFy());
1147         assertNull(calibrator.getEstimatedBiasFz());
1148         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1149         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1150         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1151         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1152         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1153         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1154         assertNull(calibrator.getEstimatedMa());
1155         assertNull(calibrator.getEstimatedSx());
1156         assertNull(calibrator.getEstimatedSy());
1157         assertNull(calibrator.getEstimatedSz());
1158         assertNull(calibrator.getEstimatedMxy());
1159         assertNull(calibrator.getEstimatedMxz());
1160         assertNull(calibrator.getEstimatedMyx());
1161         assertNull(calibrator.getEstimatedMyz());
1162         assertNull(calibrator.getEstimatedMzx());
1163         assertNull(calibrator.getEstimatedMzy());
1164         assertNull(calibrator.getEstimatedCovariance());
1165         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1166         assertNull(calibrator.getGroundTruthGravityNorm());
1167         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1168         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1169     }
1170 
1171     @Test
1172     public void testConstructor12() throws WrongSizeException {
1173         final Collection<StandardDeviationBodyKinematics> measurements =
1174                 Collections.emptyList();
1175 
1176         final Matrix ba = generateBa();
1177         final double biasX = ba.getElementAtIndex(0);
1178         final double biasY = ba.getElementAtIndex(1);
1179         final double biasZ = ba.getElementAtIndex(2);
1180 
1181         final KnownPositionAccelerometerCalibrator calibrator =
1182                 new KnownPositionAccelerometerCalibrator(measurements,
1183                         biasX, biasY, biasZ, this);
1184 
1185         // check default values
1186         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1187         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1188         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1189         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1190         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1191         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1192         final Acceleration bx2 = new Acceleration(0.0,
1193                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1194         calibrator.getInitialBiasXAsAcceleration(bx2);
1195         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1196         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1197         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1198         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1199         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1200         final Acceleration by2 = new Acceleration(0.0,
1201                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1202         calibrator.getInitialBiasYAsAcceleration(by2);
1203         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1204         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1205         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1206         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1207         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1208         final Acceleration bz2 = new Acceleration(0.0,
1209                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1210         calibrator.getInitialBiasZAsAcceleration(bz2);
1211         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1212         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1213         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1214         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1215         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1216         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1217         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1218         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1219         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1220         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1221         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1222         final double[] bias1 = calibrator.getInitialBias();
1223         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1224         final double[] bias2 = new double[3];
1225         calibrator.getInitialBias(bias2);
1226         assertArrayEquals(bias1, bias2, 0.0);
1227         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1228         assertEquals(b1, ba);
1229         final Matrix b2 = new Matrix(3, 1);
1230         calibrator.getInitialBiasAsMatrix(b2);
1231         assertEquals(b1, b2);
1232         final Matrix ma1 = calibrator.getInitialMa();
1233         assertEquals(ma1, new Matrix(3, 3));
1234         final Matrix ma2 = new Matrix(3, 3);
1235         calibrator.getInitialMa(ma2);
1236         assertEquals(ma1, ma2);
1237         assertNull(calibrator.getEcefPosition());
1238         assertNull(calibrator.getNedPosition());
1239         assertFalse(calibrator.getNedPosition(null));
1240         assertSame(calibrator.getMeasurements(), measurements);
1241         assertFalse(calibrator.isCommonAxisUsed());
1242         assertSame(calibrator.getListener(), this);
1243         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1244         assertFalse(calibrator.isReady());
1245         assertFalse(calibrator.isRunning());
1246         assertNull(calibrator.getEstimatedBiases());
1247         assertFalse(calibrator.getEstimatedBiases(null));
1248         assertNull(calibrator.getEstimatedBiasesAsMatrix());
1249         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1250         assertNull(calibrator.getEstimatedBiasFx());
1251         assertNull(calibrator.getEstimatedBiasFy());
1252         assertNull(calibrator.getEstimatedBiasFz());
1253         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1254         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1255         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1256         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1257         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1258         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1259         assertNull(calibrator.getEstimatedMa());
1260         assertNull(calibrator.getEstimatedSx());
1261         assertNull(calibrator.getEstimatedSy());
1262         assertNull(calibrator.getEstimatedSz());
1263         assertNull(calibrator.getEstimatedMxy());
1264         assertNull(calibrator.getEstimatedMxz());
1265         assertNull(calibrator.getEstimatedMyx());
1266         assertNull(calibrator.getEstimatedMyz());
1267         assertNull(calibrator.getEstimatedMzx());
1268         assertNull(calibrator.getEstimatedMzy());
1269         assertNull(calibrator.getEstimatedCovariance());
1270         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1271         assertNull(calibrator.getGroundTruthGravityNorm());
1272         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1273         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1274     }
1275 
1276     @Test
1277     public void testConstructor13() throws WrongSizeException {
1278         final Matrix ba = generateBa();
1279         final double biasX = ba.getElementAtIndex(0);
1280         final double biasY = ba.getElementAtIndex(1);
1281         final double biasZ = ba.getElementAtIndex(2);
1282 
1283         final KnownPositionAccelerometerCalibrator calibrator =
1284                 new KnownPositionAccelerometerCalibrator(true,
1285                         biasX, biasY, biasZ);
1286 
1287         // check default values
1288         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1289         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1290         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1291         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1292         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1293         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1294         final Acceleration bx2 = new Acceleration(0.0,
1295                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1296         calibrator.getInitialBiasXAsAcceleration(bx2);
1297         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1298         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1299         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1300         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1301         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1302         final Acceleration by2 = new Acceleration(0.0,
1303                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1304         calibrator.getInitialBiasYAsAcceleration(by2);
1305         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1306         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1307         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1308         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1309         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1310         final Acceleration bz2 = new Acceleration(0.0,
1311                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1312         calibrator.getInitialBiasZAsAcceleration(bz2);
1313         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1314         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1315         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1316         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1317         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1318         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1319         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1320         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1321         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1322         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1323         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1324         final double[] bias1 = calibrator.getInitialBias();
1325         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1326         final double[] bias2 = new double[3];
1327         calibrator.getInitialBias(bias2);
1328         assertArrayEquals(bias1, bias2, 0.0);
1329         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1330         assertEquals(b1, ba);
1331         final Matrix b2 = new Matrix(3, 1);
1332         calibrator.getInitialBiasAsMatrix(b2);
1333         assertEquals(b1, b2);
1334         final Matrix ma1 = calibrator.getInitialMa();
1335         assertEquals(ma1, new Matrix(3, 3));
1336         final Matrix ma2 = new Matrix(3, 3);
1337         calibrator.getInitialMa(ma2);
1338         assertEquals(ma1, ma2);
1339         assertNull(calibrator.getEcefPosition());
1340         assertNull(calibrator.getNedPosition());
1341         assertFalse(calibrator.getNedPosition(null));
1342         assertNull(calibrator.getMeasurements());
1343         assertTrue(calibrator.isCommonAxisUsed());
1344         assertNull(calibrator.getListener());
1345         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1346         assertFalse(calibrator.isReady());
1347         assertFalse(calibrator.isRunning());
1348         assertNull(calibrator.getEstimatedBiases());
1349         assertFalse(calibrator.getEstimatedBiases(null));
1350         assertNull(calibrator.getEstimatedBiasesAsMatrix());
1351         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1352         assertNull(calibrator.getEstimatedBiasFx());
1353         assertNull(calibrator.getEstimatedBiasFy());
1354         assertNull(calibrator.getEstimatedBiasFz());
1355         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1356         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1357         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1358         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1359         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1360         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1361         assertNull(calibrator.getEstimatedMa());
1362         assertNull(calibrator.getEstimatedSx());
1363         assertNull(calibrator.getEstimatedSy());
1364         assertNull(calibrator.getEstimatedSz());
1365         assertNull(calibrator.getEstimatedMxy());
1366         assertNull(calibrator.getEstimatedMxz());
1367         assertNull(calibrator.getEstimatedMyx());
1368         assertNull(calibrator.getEstimatedMyz());
1369         assertNull(calibrator.getEstimatedMzx());
1370         assertNull(calibrator.getEstimatedMzy());
1371         assertNull(calibrator.getEstimatedCovariance());
1372         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1373         assertNull(calibrator.getGroundTruthGravityNorm());
1374         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1375         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1376     }
1377 
1378     @Test
1379     public void testConstructor14() throws WrongSizeException {
1380         final Matrix ba = generateBa();
1381         final double biasX = ba.getElementAtIndex(0);
1382         final double biasY = ba.getElementAtIndex(1);
1383         final double biasZ = ba.getElementAtIndex(2);
1384 
1385         final KnownPositionAccelerometerCalibrator calibrator =
1386                 new KnownPositionAccelerometerCalibrator(true,
1387                         biasX, biasY, biasZ, this);
1388 
1389         // check default values
1390         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1391         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1392         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1393         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1394         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1395         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1396         final Acceleration bx2 = new Acceleration(0.0,
1397                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1398         calibrator.getInitialBiasXAsAcceleration(bx2);
1399         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1400         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1401         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1402         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1403         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1404         final Acceleration by2 = new Acceleration(0.0,
1405                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1406         calibrator.getInitialBiasYAsAcceleration(by2);
1407         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1408         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1409         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1410         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1411         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1412         final Acceleration bz2 = new Acceleration(0.0,
1413                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1414         calibrator.getInitialBiasZAsAcceleration(bz2);
1415         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1416         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1417         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1418         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1419         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1420         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1421         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1422         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1423         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1424         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1425         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1426         final double[] bias1 = calibrator.getInitialBias();
1427         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1428         final double[] bias2 = new double[3];
1429         calibrator.getInitialBias(bias2);
1430         assertArrayEquals(bias1, bias2, 0.0);
1431         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1432         assertEquals(b1, ba);
1433         final Matrix b2 = new Matrix(3, 1);
1434         calibrator.getInitialBiasAsMatrix(b2);
1435         assertEquals(b1, b2);
1436         final Matrix ma1 = calibrator.getInitialMa();
1437         assertEquals(ma1, new Matrix(3, 3));
1438         final Matrix ma2 = new Matrix(3, 3);
1439         calibrator.getInitialMa(ma2);
1440         assertEquals(ma1, ma2);
1441         assertNull(calibrator.getEcefPosition());
1442         assertNull(calibrator.getNedPosition());
1443         assertFalse(calibrator.getNedPosition(null));
1444         assertNull(calibrator.getMeasurements());
1445         assertTrue(calibrator.isCommonAxisUsed());
1446         assertSame(calibrator.getListener(), this);
1447         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1448         assertFalse(calibrator.isReady());
1449         assertFalse(calibrator.isRunning());
1450         assertNull(calibrator.getEstimatedBiases());
1451         assertFalse(calibrator.getEstimatedBiases(null));
1452         assertNull(calibrator.getEstimatedBiasesAsMatrix());
1453         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1454         assertNull(calibrator.getEstimatedBiasFx());
1455         assertNull(calibrator.getEstimatedBiasFy());
1456         assertNull(calibrator.getEstimatedBiasFz());
1457         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1458         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1459         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1460         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1461         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1462         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1463         assertNull(calibrator.getEstimatedMa());
1464         assertNull(calibrator.getEstimatedSx());
1465         assertNull(calibrator.getEstimatedSy());
1466         assertNull(calibrator.getEstimatedSz());
1467         assertNull(calibrator.getEstimatedMxy());
1468         assertNull(calibrator.getEstimatedMxz());
1469         assertNull(calibrator.getEstimatedMyx());
1470         assertNull(calibrator.getEstimatedMyz());
1471         assertNull(calibrator.getEstimatedMzx());
1472         assertNull(calibrator.getEstimatedMzy());
1473         assertNull(calibrator.getEstimatedCovariance());
1474         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1475         assertNull(calibrator.getGroundTruthGravityNorm());
1476         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1477         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1478     }
1479 
1480     @Test
1481     public void testConstructor15() throws WrongSizeException {
1482         final Collection<StandardDeviationBodyKinematics> measurements =
1483                 Collections.emptyList();
1484 
1485         final Matrix ba = generateBa();
1486         final double biasX = ba.getElementAtIndex(0);
1487         final double biasY = ba.getElementAtIndex(1);
1488         final double biasZ = ba.getElementAtIndex(2);
1489 
1490         final KnownPositionAccelerometerCalibrator calibrator =
1491                 new KnownPositionAccelerometerCalibrator(measurements,
1492                         true, biasX, biasY, biasZ);
1493 
1494         // check default values
1495         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1496         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1497         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1498         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1499         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1500         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1501         final Acceleration bx2 = new Acceleration(0.0,
1502                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1503         calibrator.getInitialBiasXAsAcceleration(bx2);
1504         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1505         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1506         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1507         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1508         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1509         final Acceleration by2 = new Acceleration(0.0,
1510                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1511         calibrator.getInitialBiasYAsAcceleration(by2);
1512         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1513         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1514         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1515         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1516         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1517         final Acceleration bz2 = new Acceleration(0.0,
1518                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1519         calibrator.getInitialBiasZAsAcceleration(bz2);
1520         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1521         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1522         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1523         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1524         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1525         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1526         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1527         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1528         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1529         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1530         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1531         final double[] bias1 = calibrator.getInitialBias();
1532         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1533         final double[] bias2 = new double[3];
1534         calibrator.getInitialBias(bias2);
1535         assertArrayEquals(bias1, bias2, 0.0);
1536         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1537         assertEquals(b1, ba);
1538         final Matrix b2 = new Matrix(3, 1);
1539         calibrator.getInitialBiasAsMatrix(b2);
1540         assertEquals(b1, b2);
1541         final Matrix ma1 = calibrator.getInitialMa();
1542         assertEquals(ma1, new Matrix(3, 3));
1543         final Matrix ma2 = new Matrix(3, 3);
1544         calibrator.getInitialMa(ma2);
1545         assertEquals(ma1, ma2);
1546         assertNull(calibrator.getEcefPosition());
1547         assertNull(calibrator.getNedPosition());
1548         assertFalse(calibrator.getNedPosition(null));
1549         assertSame(calibrator.getMeasurements(), measurements);
1550         assertTrue(calibrator.isCommonAxisUsed());
1551         assertNull(calibrator.getListener());
1552         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1553         assertFalse(calibrator.isReady());
1554         assertFalse(calibrator.isRunning());
1555         assertNull(calibrator.getEstimatedBiases());
1556         assertFalse(calibrator.getEstimatedBiases(null));
1557         assertNull(calibrator.getEstimatedBiasesAsMatrix());
1558         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1559         assertNull(calibrator.getEstimatedBiasFx());
1560         assertNull(calibrator.getEstimatedBiasFy());
1561         assertNull(calibrator.getEstimatedBiasFz());
1562         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1563         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1564         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1565         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1566         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1567         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1568         assertNull(calibrator.getEstimatedMa());
1569         assertNull(calibrator.getEstimatedSx());
1570         assertNull(calibrator.getEstimatedSy());
1571         assertNull(calibrator.getEstimatedSz());
1572         assertNull(calibrator.getEstimatedMxy());
1573         assertNull(calibrator.getEstimatedMxz());
1574         assertNull(calibrator.getEstimatedMyx());
1575         assertNull(calibrator.getEstimatedMyz());
1576         assertNull(calibrator.getEstimatedMzx());
1577         assertNull(calibrator.getEstimatedMzy());
1578         assertNull(calibrator.getEstimatedCovariance());
1579         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1580         assertNull(calibrator.getGroundTruthGravityNorm());
1581         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1582         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1583     }
1584 
1585     @Test
1586     public void testConstructor16() throws WrongSizeException {
1587         final Collection<StandardDeviationBodyKinematics> measurements =
1588                 Collections.emptyList();
1589 
1590         final Matrix ba = generateBa();
1591         final double biasX = ba.getElementAtIndex(0);
1592         final double biasY = ba.getElementAtIndex(1);
1593         final double biasZ = ba.getElementAtIndex(2);
1594 
1595         final KnownPositionAccelerometerCalibrator calibrator =
1596                 new KnownPositionAccelerometerCalibrator(measurements,
1597                         true, biasX, biasY, biasZ, this);
1598 
1599         // check default values
1600         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1601         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1602         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1603         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1604         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1605         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1606         final Acceleration bx2 = new Acceleration(0.0,
1607                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1608         calibrator.getInitialBiasXAsAcceleration(bx2);
1609         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1610         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1611         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1612         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1613         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1614         final Acceleration by2 = new Acceleration(0.0,
1615                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1616         calibrator.getInitialBiasYAsAcceleration(by2);
1617         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1618         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1619         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1620         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1621         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1622         final Acceleration bz2 = new Acceleration(0.0,
1623                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1624         calibrator.getInitialBiasZAsAcceleration(bz2);
1625         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1626         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1627         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1628         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1629         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1630         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1631         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1632         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1633         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1634         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1635         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1636         final double[] bias1 = calibrator.getInitialBias();
1637         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1638         final double[] bias2 = new double[3];
1639         calibrator.getInitialBias(bias2);
1640         assertArrayEquals(bias1, bias2, 0.0);
1641         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1642         assertEquals(b1, ba);
1643         final Matrix b2 = new Matrix(3, 1);
1644         calibrator.getInitialBiasAsMatrix(b2);
1645         assertEquals(b1, b2);
1646         final Matrix ma1 = calibrator.getInitialMa();
1647         assertEquals(ma1, new Matrix(3, 3));
1648         final Matrix ma2 = new Matrix(3, 3);
1649         calibrator.getInitialMa(ma2);
1650         assertEquals(ma1, ma2);
1651         assertNull(calibrator.getEcefPosition());
1652         assertNull(calibrator.getNedPosition());
1653         assertFalse(calibrator.getNedPosition(null));
1654         assertSame(calibrator.getMeasurements(), measurements);
1655         assertTrue(calibrator.isCommonAxisUsed());
1656         assertSame(calibrator.getListener(), this);
1657         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1658         assertFalse(calibrator.isReady());
1659         assertFalse(calibrator.isRunning());
1660         assertNull(calibrator.getEstimatedBiases());
1661         assertFalse(calibrator.getEstimatedBiases(null));
1662         assertNull(calibrator.getEstimatedBiasesAsMatrix());
1663         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1664         assertNull(calibrator.getEstimatedBiasFx());
1665         assertNull(calibrator.getEstimatedBiasFy());
1666         assertNull(calibrator.getEstimatedBiasFz());
1667         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1668         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1669         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1670         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1671         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1672         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1673         assertNull(calibrator.getEstimatedMa());
1674         assertNull(calibrator.getEstimatedSx());
1675         assertNull(calibrator.getEstimatedSy());
1676         assertNull(calibrator.getEstimatedSz());
1677         assertNull(calibrator.getEstimatedMxy());
1678         assertNull(calibrator.getEstimatedMxz());
1679         assertNull(calibrator.getEstimatedMyx());
1680         assertNull(calibrator.getEstimatedMyz());
1681         assertNull(calibrator.getEstimatedMzx());
1682         assertNull(calibrator.getEstimatedMzy());
1683         assertNull(calibrator.getEstimatedCovariance());
1684         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1685         assertNull(calibrator.getGroundTruthGravityNorm());
1686         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1687         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1688     }
1689 
1690     @Test
1691     public void testConstructor17() throws WrongSizeException {
1692         final Matrix ba = generateBa();
1693         final double biasX = ba.getElementAtIndex(0);
1694         final double biasY = ba.getElementAtIndex(1);
1695         final double biasZ = ba.getElementAtIndex(2);
1696 
1697         final Acceleration bx = new Acceleration(biasX,
1698                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1699         final Acceleration by = new Acceleration(biasY,
1700                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1701         final Acceleration bz = new Acceleration(biasZ,
1702                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1703 
1704         final KnownPositionAccelerometerCalibrator calibrator =
1705                 new KnownPositionAccelerometerCalibrator(bx, by, bz);
1706 
1707         // check default values
1708         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1709         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1710         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1711         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1712         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1713         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1714         final Acceleration bx2 = new Acceleration(0.0,
1715                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1716         calibrator.getInitialBiasXAsAcceleration(bx2);
1717         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1718         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1719         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1720         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1721         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1722         final Acceleration by2 = new Acceleration(0.0,
1723                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1724         calibrator.getInitialBiasYAsAcceleration(by2);
1725         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1726         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1727         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1728         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1729         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1730         final Acceleration bz2 = new Acceleration(0.0,
1731                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1732         calibrator.getInitialBiasZAsAcceleration(bz2);
1733         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1734         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1735         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1736         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1737         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1738         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1739         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1740         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1741         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1742         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1743         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1744         final double[] bias1 = calibrator.getInitialBias();
1745         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1746         final double[] bias2 = new double[3];
1747         calibrator.getInitialBias(bias2);
1748         assertArrayEquals(bias1, bias2, 0.0);
1749         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1750         assertEquals(b1, ba);
1751         final Matrix b2 = new Matrix(3, 1);
1752         calibrator.getInitialBiasAsMatrix(b2);
1753         assertEquals(b1, b2);
1754         final Matrix ma1 = calibrator.getInitialMa();
1755         assertEquals(ma1, new Matrix(3, 3));
1756         final Matrix ma2 = new Matrix(3, 3);
1757         calibrator.getInitialMa(ma2);
1758         assertEquals(ma1, ma2);
1759         assertNull(calibrator.getEcefPosition());
1760         assertNull(calibrator.getNedPosition());
1761         assertFalse(calibrator.getNedPosition(null));
1762         assertNull(calibrator.getMeasurements());
1763         assertFalse(calibrator.isCommonAxisUsed());
1764         assertNull(calibrator.getListener());
1765         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1766         assertFalse(calibrator.isReady());
1767         assertFalse(calibrator.isRunning());
1768         assertNull(calibrator.getEstimatedBiases());
1769         assertFalse(calibrator.getEstimatedBiases(null));
1770         assertNull(calibrator.getEstimatedBiasesAsMatrix());
1771         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1772         assertNull(calibrator.getEstimatedBiasFx());
1773         assertNull(calibrator.getEstimatedBiasFy());
1774         assertNull(calibrator.getEstimatedBiasFz());
1775         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1776         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1777         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1778         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1779         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1780         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1781         assertNull(calibrator.getEstimatedMa());
1782         assertNull(calibrator.getEstimatedSx());
1783         assertNull(calibrator.getEstimatedSy());
1784         assertNull(calibrator.getEstimatedSz());
1785         assertNull(calibrator.getEstimatedMxy());
1786         assertNull(calibrator.getEstimatedMxz());
1787         assertNull(calibrator.getEstimatedMyx());
1788         assertNull(calibrator.getEstimatedMyz());
1789         assertNull(calibrator.getEstimatedMzx());
1790         assertNull(calibrator.getEstimatedMzy());
1791         assertNull(calibrator.getEstimatedCovariance());
1792         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1793         assertNull(calibrator.getGroundTruthGravityNorm());
1794         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1795         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1796     }
1797 
1798     @Test
1799     public void testConstructor18() throws WrongSizeException {
1800         final Matrix ba = generateBa();
1801         final double biasX = ba.getElementAtIndex(0);
1802         final double biasY = ba.getElementAtIndex(1);
1803         final double biasZ = ba.getElementAtIndex(2);
1804 
1805         final Acceleration bx = new Acceleration(biasX,
1806                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1807         final Acceleration by = new Acceleration(biasY,
1808                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1809         final Acceleration bz = new Acceleration(biasZ,
1810                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1811 
1812         final KnownPositionAccelerometerCalibrator calibrator =
1813                 new KnownPositionAccelerometerCalibrator(bx, by, bz,
1814                         this);
1815 
1816         // check default values
1817         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1818         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1819         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1820         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1821         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1822         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1823         final Acceleration bx2 = new Acceleration(0.0,
1824                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1825         calibrator.getInitialBiasXAsAcceleration(bx2);
1826         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1827         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1828         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1829         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1830         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1831         final Acceleration by2 = new Acceleration(0.0,
1832                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1833         calibrator.getInitialBiasYAsAcceleration(by2);
1834         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1835         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1836         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1837         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1838         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1839         final Acceleration bz2 = new Acceleration(0.0,
1840                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1841         calibrator.getInitialBiasZAsAcceleration(bz2);
1842         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1843         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1844         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1845         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1846         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1847         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1848         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1849         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1850         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1851         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1852         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1853         final double[] bias1 = calibrator.getInitialBias();
1854         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1855         final double[] bias2 = new double[3];
1856         calibrator.getInitialBias(bias2);
1857         assertArrayEquals(bias1, bias2, 0.0);
1858         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1859         assertEquals(b1, ba);
1860         final Matrix b2 = new Matrix(3, 1);
1861         calibrator.getInitialBiasAsMatrix(b2);
1862         assertEquals(b1, b2);
1863         final Matrix ma1 = calibrator.getInitialMa();
1864         assertEquals(ma1, new Matrix(3, 3));
1865         final Matrix ma2 = new Matrix(3, 3);
1866         calibrator.getInitialMa(ma2);
1867         assertEquals(ma1, ma2);
1868         assertNull(calibrator.getEcefPosition());
1869         assertNull(calibrator.getNedPosition());
1870         assertFalse(calibrator.getNedPosition(null));
1871         assertNull(calibrator.getMeasurements());
1872         assertFalse(calibrator.isCommonAxisUsed());
1873         assertSame(calibrator.getListener(), this);
1874         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1875         assertFalse(calibrator.isReady());
1876         assertFalse(calibrator.isRunning());
1877         assertNull(calibrator.getEstimatedBiases());
1878         assertFalse(calibrator.getEstimatedBiases(null));
1879         assertNull(calibrator.getEstimatedBiasesAsMatrix());
1880         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1881         assertNull(calibrator.getEstimatedBiasFx());
1882         assertNull(calibrator.getEstimatedBiasFy());
1883         assertNull(calibrator.getEstimatedBiasFz());
1884         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1885         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1886         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1887         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1888         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1889         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1890         assertNull(calibrator.getEstimatedMa());
1891         assertNull(calibrator.getEstimatedSx());
1892         assertNull(calibrator.getEstimatedSy());
1893         assertNull(calibrator.getEstimatedSz());
1894         assertNull(calibrator.getEstimatedMxy());
1895         assertNull(calibrator.getEstimatedMxz());
1896         assertNull(calibrator.getEstimatedMyx());
1897         assertNull(calibrator.getEstimatedMyz());
1898         assertNull(calibrator.getEstimatedMzx());
1899         assertNull(calibrator.getEstimatedMzy());
1900         assertNull(calibrator.getEstimatedCovariance());
1901         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1902         assertNull(calibrator.getGroundTruthGravityNorm());
1903         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1904         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1905     }
1906 
1907     @Test
1908     public void testConstructor19() throws WrongSizeException {
1909         final Collection<StandardDeviationBodyKinematics> measurements =
1910                 Collections.emptyList();
1911 
1912         final Matrix ba = generateBa();
1913         final double biasX = ba.getElementAtIndex(0);
1914         final double biasY = ba.getElementAtIndex(1);
1915         final double biasZ = ba.getElementAtIndex(2);
1916 
1917         final Acceleration bx = new Acceleration(biasX,
1918                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1919         final Acceleration by = new Acceleration(biasY,
1920                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1921         final Acceleration bz = new Acceleration(biasZ,
1922                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1923 
1924         final KnownPositionAccelerometerCalibrator calibrator =
1925                 new KnownPositionAccelerometerCalibrator(measurements,
1926                         bx, by, bz);
1927 
1928         // check default values
1929         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1930         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1931         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1932         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1933         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1934         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1935         final Acceleration bx2 = new Acceleration(0.0,
1936                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1937         calibrator.getInitialBiasXAsAcceleration(bx2);
1938         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1939         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1940         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1941         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1942         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1943         final Acceleration by2 = new Acceleration(0.0,
1944                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1945         calibrator.getInitialBiasYAsAcceleration(by2);
1946         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1947         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1948         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1949         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1950         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1951         final Acceleration bz2 = new Acceleration(0.0,
1952                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1953         calibrator.getInitialBiasZAsAcceleration(bz2);
1954         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1955         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1956         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1957         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1958         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1959         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1960         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1961         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1962         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1963         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1964         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1965         final double[] bias1 = calibrator.getInitialBias();
1966         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1967         final double[] bias2 = new double[3];
1968         calibrator.getInitialBias(bias2);
1969         assertArrayEquals(bias1, bias2, 0.0);
1970         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1971         assertEquals(b1, ba);
1972         final Matrix b2 = new Matrix(3, 1);
1973         calibrator.getInitialBiasAsMatrix(b2);
1974         assertEquals(b1, b2);
1975         final Matrix ma1 = calibrator.getInitialMa();
1976         assertEquals(ma1, new Matrix(3, 3));
1977         final Matrix ma2 = new Matrix(3, 3);
1978         calibrator.getInitialMa(ma2);
1979         assertEquals(ma1, ma2);
1980         assertNull(calibrator.getEcefPosition());
1981         assertNull(calibrator.getNedPosition());
1982         assertFalse(calibrator.getNedPosition(null));
1983         assertSame(calibrator.getMeasurements(), measurements);
1984         assertFalse(calibrator.isCommonAxisUsed());
1985         assertNull(calibrator.getListener());
1986         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1987         assertFalse(calibrator.isReady());
1988         assertFalse(calibrator.isRunning());
1989         assertNull(calibrator.getEstimatedBiases());
1990         assertFalse(calibrator.getEstimatedBiases(null));
1991         assertNull(calibrator.getEstimatedBiasesAsMatrix());
1992         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1993         assertNull(calibrator.getEstimatedBiasFx());
1994         assertNull(calibrator.getEstimatedBiasFy());
1995         assertNull(calibrator.getEstimatedBiasFz());
1996         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1997         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1998         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1999         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2000         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2001         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2002         assertNull(calibrator.getEstimatedMa());
2003         assertNull(calibrator.getEstimatedSx());
2004         assertNull(calibrator.getEstimatedSy());
2005         assertNull(calibrator.getEstimatedSz());
2006         assertNull(calibrator.getEstimatedMxy());
2007         assertNull(calibrator.getEstimatedMxz());
2008         assertNull(calibrator.getEstimatedMyx());
2009         assertNull(calibrator.getEstimatedMyz());
2010         assertNull(calibrator.getEstimatedMzx());
2011         assertNull(calibrator.getEstimatedMzy());
2012         assertNull(calibrator.getEstimatedCovariance());
2013         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2014         assertNull(calibrator.getGroundTruthGravityNorm());
2015         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2016         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2017     }
2018 
2019     @Test
2020     public void testConstructor20() throws WrongSizeException {
2021         final Collection<StandardDeviationBodyKinematics> measurements =
2022                 Collections.emptyList();
2023 
2024         final Matrix ba = generateBa();
2025         final double biasX = ba.getElementAtIndex(0);
2026         final double biasY = ba.getElementAtIndex(1);
2027         final double biasZ = ba.getElementAtIndex(2);
2028 
2029         final Acceleration bx = new Acceleration(biasX,
2030                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2031         final Acceleration by = new Acceleration(biasY,
2032                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2033         final Acceleration bz = new Acceleration(biasZ,
2034                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2035 
2036         final KnownPositionAccelerometerCalibrator calibrator =
2037                 new KnownPositionAccelerometerCalibrator(measurements,
2038                         bx, by, bz, this);
2039 
2040         // check default values
2041         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2042         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2043         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2044         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2045         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2046         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2047         final Acceleration bx2 = new Acceleration(0.0,
2048                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2049         calibrator.getInitialBiasXAsAcceleration(bx2);
2050         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2051         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2052         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2053         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2054         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2055         final Acceleration by2 = new Acceleration(0.0,
2056                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2057         calibrator.getInitialBiasYAsAcceleration(by2);
2058         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2059         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2060         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2061         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2062         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2063         final Acceleration bz2 = new Acceleration(0.0,
2064                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2065         calibrator.getInitialBiasZAsAcceleration(bz2);
2066         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2067         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2068         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
2069         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
2070         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2071         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2072         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2073         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2074         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2075         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2076         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2077         final double[] bias1 = calibrator.getInitialBias();
2078         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2079         final double[] bias2 = new double[3];
2080         calibrator.getInitialBias(bias2);
2081         assertArrayEquals(bias1, bias2, 0.0);
2082         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2083         assertEquals(b1, ba);
2084         final Matrix b2 = new Matrix(3, 1);
2085         calibrator.getInitialBiasAsMatrix(b2);
2086         assertEquals(b1, b2);
2087         final Matrix ma1 = calibrator.getInitialMa();
2088         assertEquals(ma1, new Matrix(3, 3));
2089         final Matrix ma2 = new Matrix(3, 3);
2090         calibrator.getInitialMa(ma2);
2091         assertEquals(ma1, ma2);
2092         assertNull(calibrator.getEcefPosition());
2093         assertNull(calibrator.getNedPosition());
2094         assertFalse(calibrator.getNedPosition(null));
2095         assertSame(calibrator.getMeasurements(), measurements);
2096         assertFalse(calibrator.isCommonAxisUsed());
2097         assertSame(calibrator.getListener(), this);
2098         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
2099         assertFalse(calibrator.isReady());
2100         assertFalse(calibrator.isRunning());
2101         assertNull(calibrator.getEstimatedBiases());
2102         assertFalse(calibrator.getEstimatedBiases(null));
2103         assertNull(calibrator.getEstimatedBiasesAsMatrix());
2104         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2105         assertNull(calibrator.getEstimatedBiasFx());
2106         assertNull(calibrator.getEstimatedBiasFy());
2107         assertNull(calibrator.getEstimatedBiasFz());
2108         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2109         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2110         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2111         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2112         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2113         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2114         assertNull(calibrator.getEstimatedMa());
2115         assertNull(calibrator.getEstimatedSx());
2116         assertNull(calibrator.getEstimatedSy());
2117         assertNull(calibrator.getEstimatedSz());
2118         assertNull(calibrator.getEstimatedMxy());
2119         assertNull(calibrator.getEstimatedMxz());
2120         assertNull(calibrator.getEstimatedMyx());
2121         assertNull(calibrator.getEstimatedMyz());
2122         assertNull(calibrator.getEstimatedMzx());
2123         assertNull(calibrator.getEstimatedMzy());
2124         assertNull(calibrator.getEstimatedCovariance());
2125         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2126         assertNull(calibrator.getGroundTruthGravityNorm());
2127         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2128         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2129     }
2130 
2131     @Test
2132     public void testConstructor21() throws WrongSizeException {
2133         final Matrix ba = generateBa();
2134         final double biasX = ba.getElementAtIndex(0);
2135         final double biasY = ba.getElementAtIndex(1);
2136         final double biasZ = ba.getElementAtIndex(2);
2137 
2138         final Acceleration bx = new Acceleration(biasX,
2139                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2140         final Acceleration by = new Acceleration(biasY,
2141                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2142         final Acceleration bz = new Acceleration(biasZ,
2143                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2144 
2145         final KnownPositionAccelerometerCalibrator calibrator =
2146                 new KnownPositionAccelerometerCalibrator(true,
2147                         bx, by, bz);
2148 
2149         // check default values
2150         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2151         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2152         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2153         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2154         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2155         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2156         final Acceleration bx2 = new Acceleration(0.0,
2157                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2158         calibrator.getInitialBiasXAsAcceleration(bx2);
2159         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2160         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2161         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2162         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2163         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2164         final Acceleration by2 = new Acceleration(0.0,
2165                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2166         calibrator.getInitialBiasYAsAcceleration(by2);
2167         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2168         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2169         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2170         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2171         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2172         final Acceleration bz2 = new Acceleration(0.0,
2173                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2174         calibrator.getInitialBiasZAsAcceleration(bz2);
2175         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2176         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2177         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
2178         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
2179         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2180         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2181         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2182         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2183         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2184         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2185         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2186         final double[] bias1 = calibrator.getInitialBias();
2187         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2188         final double[] bias2 = new double[3];
2189         calibrator.getInitialBias(bias2);
2190         assertArrayEquals(bias1, bias2, 0.0);
2191         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2192         assertEquals(b1, ba);
2193         final Matrix b2 = new Matrix(3, 1);
2194         calibrator.getInitialBiasAsMatrix(b2);
2195         assertEquals(b1, b2);
2196         final Matrix ma1 = calibrator.getInitialMa();
2197         assertEquals(ma1, new Matrix(3, 3));
2198         final Matrix ma2 = new Matrix(3, 3);
2199         calibrator.getInitialMa(ma2);
2200         assertEquals(ma1, ma2);
2201         assertNull(calibrator.getEcefPosition());
2202         assertNull(calibrator.getNedPosition());
2203         assertFalse(calibrator.getNedPosition(null));
2204         assertNull(calibrator.getMeasurements());
2205         assertTrue(calibrator.isCommonAxisUsed());
2206         assertNull(calibrator.getListener());
2207         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2208         assertFalse(calibrator.isReady());
2209         assertFalse(calibrator.isRunning());
2210         assertNull(calibrator.getEstimatedBiases());
2211         assertFalse(calibrator.getEstimatedBiases(null));
2212         assertNull(calibrator.getEstimatedBiasesAsMatrix());
2213         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2214         assertNull(calibrator.getEstimatedBiasFx());
2215         assertNull(calibrator.getEstimatedBiasFy());
2216         assertNull(calibrator.getEstimatedBiasFz());
2217         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2218         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2219         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2220         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2221         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2222         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2223         assertNull(calibrator.getEstimatedMa());
2224         assertNull(calibrator.getEstimatedSx());
2225         assertNull(calibrator.getEstimatedSy());
2226         assertNull(calibrator.getEstimatedSz());
2227         assertNull(calibrator.getEstimatedMxy());
2228         assertNull(calibrator.getEstimatedMxz());
2229         assertNull(calibrator.getEstimatedMyx());
2230         assertNull(calibrator.getEstimatedMyz());
2231         assertNull(calibrator.getEstimatedMzx());
2232         assertNull(calibrator.getEstimatedMzy());
2233         assertNull(calibrator.getEstimatedCovariance());
2234         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2235         assertNull(calibrator.getGroundTruthGravityNorm());
2236         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2237         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2238     }
2239 
2240     @Test
2241     public void testConstructor22() throws WrongSizeException {
2242         final Matrix ba = generateBa();
2243         final double biasX = ba.getElementAtIndex(0);
2244         final double biasY = ba.getElementAtIndex(1);
2245         final double biasZ = ba.getElementAtIndex(2);
2246 
2247         final Acceleration bx = new Acceleration(biasX,
2248                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2249         final Acceleration by = new Acceleration(biasY,
2250                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2251         final Acceleration bz = new Acceleration(biasZ,
2252                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2253 
2254         final KnownPositionAccelerometerCalibrator calibrator =
2255                 new KnownPositionAccelerometerCalibrator(true,
2256                         bx, by, bz, this);
2257 
2258         // check default values
2259         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2260         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2261         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2262         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2263         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2264         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2265         final Acceleration bx2 = new Acceleration(0.0,
2266                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2267         calibrator.getInitialBiasXAsAcceleration(bx2);
2268         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2269         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2270         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2271         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2272         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2273         final Acceleration by2 = new Acceleration(0.0,
2274                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2275         calibrator.getInitialBiasYAsAcceleration(by2);
2276         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2277         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2278         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2279         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2280         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2281         final Acceleration bz2 = new Acceleration(0.0,
2282                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2283         calibrator.getInitialBiasZAsAcceleration(bz2);
2284         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2285         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2286         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
2287         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
2288         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2289         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2290         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2291         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2292         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2293         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2294         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2295         final double[] bias1 = calibrator.getInitialBias();
2296         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2297         final double[] bias2 = new double[3];
2298         calibrator.getInitialBias(bias2);
2299         assertArrayEquals(bias1, bias2, 0.0);
2300         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2301         assertEquals(b1, ba);
2302         final Matrix b2 = new Matrix(3, 1);
2303         calibrator.getInitialBiasAsMatrix(b2);
2304         assertEquals(b1, b2);
2305         final Matrix ma1 = calibrator.getInitialMa();
2306         assertEquals(ma1, new Matrix(3, 3));
2307         final Matrix ma2 = new Matrix(3, 3);
2308         calibrator.getInitialMa(ma2);
2309         assertEquals(ma1, ma2);
2310         assertNull(calibrator.getEcefPosition());
2311         assertNull(calibrator.getNedPosition());
2312         assertFalse(calibrator.getNedPosition(null));
2313         assertNull(calibrator.getMeasurements());
2314         assertTrue(calibrator.isCommonAxisUsed());
2315         assertSame(calibrator.getListener(), this);
2316         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2317         assertFalse(calibrator.isReady());
2318         assertFalse(calibrator.isRunning());
2319         assertNull(calibrator.getEstimatedBiases());
2320         assertFalse(calibrator.getEstimatedBiases(null));
2321         assertNull(calibrator.getEstimatedBiasesAsMatrix());
2322         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2323         assertNull(calibrator.getEstimatedBiasFx());
2324         assertNull(calibrator.getEstimatedBiasFy());
2325         assertNull(calibrator.getEstimatedBiasFz());
2326         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2327         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2328         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2329         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2330         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2331         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2332         assertNull(calibrator.getEstimatedMa());
2333         assertNull(calibrator.getEstimatedSx());
2334         assertNull(calibrator.getEstimatedSy());
2335         assertNull(calibrator.getEstimatedSz());
2336         assertNull(calibrator.getEstimatedMxy());
2337         assertNull(calibrator.getEstimatedMxz());
2338         assertNull(calibrator.getEstimatedMyx());
2339         assertNull(calibrator.getEstimatedMyz());
2340         assertNull(calibrator.getEstimatedMzx());
2341         assertNull(calibrator.getEstimatedMzy());
2342         assertNull(calibrator.getEstimatedCovariance());
2343         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2344         assertNull(calibrator.getGroundTruthGravityNorm());
2345         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2346         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2347     }
2348 
2349     @Test
2350     public void testConstructor23() throws WrongSizeException {
2351         final Collection<StandardDeviationBodyKinematics> measurements =
2352                 Collections.emptyList();
2353 
2354         final Matrix ba = generateBa();
2355         final double biasX = ba.getElementAtIndex(0);
2356         final double biasY = ba.getElementAtIndex(1);
2357         final double biasZ = ba.getElementAtIndex(2);
2358 
2359         final Acceleration bx = new Acceleration(biasX,
2360                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2361         final Acceleration by = new Acceleration(biasY,
2362                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2363         final Acceleration bz = new Acceleration(biasZ,
2364                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2365 
2366         final KnownPositionAccelerometerCalibrator calibrator =
2367                 new KnownPositionAccelerometerCalibrator(measurements,
2368                         true, bx, by, bz);
2369 
2370         // check default values
2371         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2372         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2373         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2374         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2375         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2376         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2377         final Acceleration bx2 = new Acceleration(0.0,
2378                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2379         calibrator.getInitialBiasXAsAcceleration(bx2);
2380         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2381         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2382         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2383         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2384         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2385         final Acceleration by2 = new Acceleration(0.0,
2386                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2387         calibrator.getInitialBiasYAsAcceleration(by2);
2388         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2389         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2390         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2391         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2392         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2393         final Acceleration bz2 = new Acceleration(0.0,
2394                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2395         calibrator.getInitialBiasZAsAcceleration(bz2);
2396         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2397         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2398         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
2399         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
2400         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2401         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2402         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2403         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2404         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2405         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2406         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2407         final double[] bias1 = calibrator.getInitialBias();
2408         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2409         final double[] bias2 = new double[3];
2410         calibrator.getInitialBias(bias2);
2411         assertArrayEquals(bias1, bias2, 0.0);
2412         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2413         assertEquals(b1, ba);
2414         final Matrix b2 = new Matrix(3, 1);
2415         calibrator.getInitialBiasAsMatrix(b2);
2416         assertEquals(b1, b2);
2417         final Matrix ma1 = calibrator.getInitialMa();
2418         assertEquals(ma1, new Matrix(3, 3));
2419         final Matrix ma2 = new Matrix(3, 3);
2420         calibrator.getInitialMa(ma2);
2421         assertEquals(ma1, ma2);
2422         assertNull(calibrator.getEcefPosition());
2423         assertNull(calibrator.getNedPosition());
2424         assertFalse(calibrator.getNedPosition(null));
2425         assertSame(calibrator.getMeasurements(), measurements);
2426         assertTrue(calibrator.isCommonAxisUsed());
2427         assertNull(calibrator.getListener());
2428         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2429         assertFalse(calibrator.isReady());
2430         assertFalse(calibrator.isRunning());
2431         assertNull(calibrator.getEstimatedBiases());
2432         assertFalse(calibrator.getEstimatedBiases(null));
2433         assertNull(calibrator.getEstimatedBiasesAsMatrix());
2434         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2435         assertNull(calibrator.getEstimatedBiasFx());
2436         assertNull(calibrator.getEstimatedBiasFy());
2437         assertNull(calibrator.getEstimatedBiasFz());
2438         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2439         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2440         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2441         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2442         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2443         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2444         assertNull(calibrator.getEstimatedMa());
2445         assertNull(calibrator.getEstimatedSx());
2446         assertNull(calibrator.getEstimatedSy());
2447         assertNull(calibrator.getEstimatedSz());
2448         assertNull(calibrator.getEstimatedMxy());
2449         assertNull(calibrator.getEstimatedMxz());
2450         assertNull(calibrator.getEstimatedMyx());
2451         assertNull(calibrator.getEstimatedMyz());
2452         assertNull(calibrator.getEstimatedMzx());
2453         assertNull(calibrator.getEstimatedMzy());
2454         assertNull(calibrator.getEstimatedCovariance());
2455         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2456         assertNull(calibrator.getGroundTruthGravityNorm());
2457         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2458         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2459     }
2460 
2461     @Test
2462     public void testConstructor24() throws WrongSizeException {
2463         final Collection<StandardDeviationBodyKinematics> measurements =
2464                 Collections.emptyList();
2465 
2466         final Matrix ba = generateBa();
2467         final double biasX = ba.getElementAtIndex(0);
2468         final double biasY = ba.getElementAtIndex(1);
2469         final double biasZ = ba.getElementAtIndex(2);
2470 
2471         final Acceleration bx = new Acceleration(biasX,
2472                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2473         final Acceleration by = new Acceleration(biasY,
2474                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2475         final Acceleration bz = new Acceleration(biasZ,
2476                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2477 
2478         final KnownPositionAccelerometerCalibrator calibrator =
2479                 new KnownPositionAccelerometerCalibrator(measurements,
2480                         true, bx, by, bz, this);
2481 
2482         // check default values
2483         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2484         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2485         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2486         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2487         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2488         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2489         final Acceleration bx2 = new Acceleration(0.0,
2490                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2491         calibrator.getInitialBiasXAsAcceleration(bx2);
2492         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2493         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2494         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2495         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2496         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2497         final Acceleration by2 = new Acceleration(0.0,
2498                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2499         calibrator.getInitialBiasYAsAcceleration(by2);
2500         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2501         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2502         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2503         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2504         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2505         final Acceleration bz2 = new Acceleration(0.0,
2506                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2507         calibrator.getInitialBiasZAsAcceleration(bz2);
2508         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2509         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2510         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
2511         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
2512         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2513         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2514         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2515         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2516         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2517         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2518         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2519         final double[] bias1 = calibrator.getInitialBias();
2520         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2521         final double[] bias2 = new double[3];
2522         calibrator.getInitialBias(bias2);
2523         assertArrayEquals(bias1, bias2, 0.0);
2524         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2525         assertEquals(b1, ba);
2526         final Matrix b2 = new Matrix(3, 1);
2527         calibrator.getInitialBiasAsMatrix(b2);
2528         assertEquals(b1, b2);
2529         final Matrix ma1 = calibrator.getInitialMa();
2530         assertEquals(ma1, new Matrix(3, 3));
2531         final Matrix ma2 = new Matrix(3, 3);
2532         calibrator.getInitialMa(ma2);
2533         assertEquals(ma1, ma2);
2534         assertNull(calibrator.getEcefPosition());
2535         assertNull(calibrator.getNedPosition());
2536         assertFalse(calibrator.getNedPosition(null));
2537         assertSame(calibrator.getMeasurements(), measurements);
2538         assertTrue(calibrator.isCommonAxisUsed());
2539         assertSame(calibrator.getListener(), this);
2540         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2541         assertFalse(calibrator.isReady());
2542         assertFalse(calibrator.isRunning());
2543         assertNull(calibrator.getEstimatedBiases());
2544         assertFalse(calibrator.getEstimatedBiases(null));
2545         assertNull(calibrator.getEstimatedBiasesAsMatrix());
2546         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2547         assertNull(calibrator.getEstimatedBiasFx());
2548         assertNull(calibrator.getEstimatedBiasFy());
2549         assertNull(calibrator.getEstimatedBiasFz());
2550         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2551         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2552         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2553         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2554         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2555         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2556         assertNull(calibrator.getEstimatedMa());
2557         assertNull(calibrator.getEstimatedSx());
2558         assertNull(calibrator.getEstimatedSy());
2559         assertNull(calibrator.getEstimatedSz());
2560         assertNull(calibrator.getEstimatedMxy());
2561         assertNull(calibrator.getEstimatedMxz());
2562         assertNull(calibrator.getEstimatedMyx());
2563         assertNull(calibrator.getEstimatedMyz());
2564         assertNull(calibrator.getEstimatedMzx());
2565         assertNull(calibrator.getEstimatedMzy());
2566         assertNull(calibrator.getEstimatedCovariance());
2567         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2568         assertNull(calibrator.getGroundTruthGravityNorm());
2569         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2570         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2571     }
2572 
2573     @Test
2574     public void testConstructor25() throws WrongSizeException {
2575         final Matrix ba = generateBa();
2576         final double biasX = ba.getElementAtIndex(0);
2577         final double biasY = ba.getElementAtIndex(1);
2578         final double biasZ = ba.getElementAtIndex(2);
2579 
2580         final Matrix ma = generateMaCommonAxis();
2581         final double sx = ma.getElementAt(0, 0);
2582         final double sy = ma.getElementAt(1, 1);
2583         final double sz = ma.getElementAt(2, 2);
2584 
2585         final KnownPositionAccelerometerCalibrator calibrator =
2586                 new KnownPositionAccelerometerCalibrator(biasX, biasY, biasZ,
2587                         sx, sy, sz);
2588 
2589         // check default values
2590         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2591         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2592         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2593         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2594         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2595         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2596         final Acceleration bx2 = new Acceleration(0.0,
2597                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2598         calibrator.getInitialBiasXAsAcceleration(bx2);
2599         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2600         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2601         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2602         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2603         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2604         final Acceleration by2 = new Acceleration(0.0,
2605                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2606         calibrator.getInitialBiasYAsAcceleration(by2);
2607         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2608         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2609         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2610         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2611         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2612         final Acceleration bz2 = new Acceleration(0.0,
2613                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2614         calibrator.getInitialBiasZAsAcceleration(bz2);
2615         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2616         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2617         assertEquals(calibrator.getInitialSx(), sx, 0.0);
2618         assertEquals(calibrator.getInitialSy(), sy, 0.0);
2619         assertEquals(calibrator.getInitialSz(), sz, 0.0);
2620         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2621         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2622         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2623         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2624         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2625         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2626         final double[] bias1 = calibrator.getInitialBias();
2627         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2628         final double[] bias2 = new double[3];
2629         calibrator.getInitialBias(bias2);
2630         assertArrayEquals(bias1, bias2, 0.0);
2631         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2632         assertEquals(b1, ba);
2633         final Matrix b2 = new Matrix(3, 1);
2634         calibrator.getInitialBiasAsMatrix(b2);
2635         assertEquals(b1, b2);
2636         final Matrix ma1 = calibrator.getInitialMa();
2637         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2638         final Matrix ma2 = new Matrix(3, 3);
2639         calibrator.getInitialMa(ma2);
2640         assertEquals(ma1, ma2);
2641         assertNull(calibrator.getEcefPosition());
2642         assertNull(calibrator.getNedPosition());
2643         assertFalse(calibrator.getNedPosition(null));
2644         assertNull(calibrator.getMeasurements());
2645         assertFalse(calibrator.isCommonAxisUsed());
2646         assertNull(calibrator.getListener());
2647         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
2648         assertFalse(calibrator.isReady());
2649         assertFalse(calibrator.isRunning());
2650         assertNull(calibrator.getEstimatedBiases());
2651         assertFalse(calibrator.getEstimatedBiases(null));
2652         assertNull(calibrator.getEstimatedBiasesAsMatrix());
2653         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2654         assertNull(calibrator.getEstimatedBiasFx());
2655         assertNull(calibrator.getEstimatedBiasFy());
2656         assertNull(calibrator.getEstimatedBiasFz());
2657         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2658         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2659         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2660         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2661         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2662         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2663         assertNull(calibrator.getEstimatedMa());
2664         assertNull(calibrator.getEstimatedSx());
2665         assertNull(calibrator.getEstimatedSy());
2666         assertNull(calibrator.getEstimatedSz());
2667         assertNull(calibrator.getEstimatedMxy());
2668         assertNull(calibrator.getEstimatedMxz());
2669         assertNull(calibrator.getEstimatedMyx());
2670         assertNull(calibrator.getEstimatedMyz());
2671         assertNull(calibrator.getEstimatedMzx());
2672         assertNull(calibrator.getEstimatedMzy());
2673         assertNull(calibrator.getEstimatedCovariance());
2674         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2675         assertNull(calibrator.getGroundTruthGravityNorm());
2676         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2677         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2678     }
2679 
2680     @Test
2681     public void testConstructor26() throws WrongSizeException {
2682         final Collection<StandardDeviationBodyKinematics> measurements =
2683                 Collections.emptyList();
2684 
2685         final Matrix ba = generateBa();
2686         final double biasX = ba.getElementAtIndex(0);
2687         final double biasY = ba.getElementAtIndex(1);
2688         final double biasZ = ba.getElementAtIndex(2);
2689 
2690         final Matrix ma = generateMaCommonAxis();
2691         final double sx = ma.getElementAt(0, 0);
2692         final double sy = ma.getElementAt(1, 1);
2693         final double sz = ma.getElementAt(2, 2);
2694 
2695         final KnownPositionAccelerometerCalibrator calibrator =
2696                 new KnownPositionAccelerometerCalibrator(measurements,
2697                         biasX, biasY, biasZ, sx, sy, sz);
2698 
2699         // check default values
2700         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2701         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2702         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2703         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2704         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2705         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2706         final Acceleration bx2 = new Acceleration(0.0,
2707                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2708         calibrator.getInitialBiasXAsAcceleration(bx2);
2709         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2710         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2711         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2712         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2713         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2714         final Acceleration by2 = new Acceleration(0.0,
2715                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2716         calibrator.getInitialBiasYAsAcceleration(by2);
2717         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2718         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2719         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2720         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2721         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2722         final Acceleration bz2 = new Acceleration(0.0,
2723                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2724         calibrator.getInitialBiasZAsAcceleration(bz2);
2725         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2726         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2727         assertEquals(calibrator.getInitialSx(), sx, 0.0);
2728         assertEquals(calibrator.getInitialSy(), sy, 0.0);
2729         assertEquals(calibrator.getInitialSz(), sz, 0.0);
2730         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2731         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2732         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2733         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2734         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2735         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2736         final double[] bias1 = calibrator.getInitialBias();
2737         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2738         final double[] bias2 = new double[3];
2739         calibrator.getInitialBias(bias2);
2740         assertArrayEquals(bias1, bias2, 0.0);
2741         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2742         assertEquals(b1, ba);
2743         final Matrix b2 = new Matrix(3, 1);
2744         calibrator.getInitialBiasAsMatrix(b2);
2745         assertEquals(b1, b2);
2746         final Matrix ma1 = calibrator.getInitialMa();
2747         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2748         final Matrix ma2 = new Matrix(3, 3);
2749         calibrator.getInitialMa(ma2);
2750         assertEquals(ma1, ma2);
2751         assertNull(calibrator.getEcefPosition());
2752         assertNull(calibrator.getNedPosition());
2753         assertFalse(calibrator.getNedPosition(null));
2754         assertSame(calibrator.getMeasurements(), measurements);
2755         assertFalse(calibrator.isCommonAxisUsed());
2756         assertNull(calibrator.getListener());
2757         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
2758         assertFalse(calibrator.isReady());
2759         assertFalse(calibrator.isRunning());
2760         assertNull(calibrator.getEstimatedBiases());
2761         assertFalse(calibrator.getEstimatedBiases(null));
2762         assertNull(calibrator.getEstimatedBiasesAsMatrix());
2763         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2764         assertNull(calibrator.getEstimatedBiasFx());
2765         assertNull(calibrator.getEstimatedBiasFy());
2766         assertNull(calibrator.getEstimatedBiasFz());
2767         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2768         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2769         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2770         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2771         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2772         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2773         assertNull(calibrator.getEstimatedMa());
2774         assertNull(calibrator.getEstimatedSx());
2775         assertNull(calibrator.getEstimatedSy());
2776         assertNull(calibrator.getEstimatedSz());
2777         assertNull(calibrator.getEstimatedMxy());
2778         assertNull(calibrator.getEstimatedMxz());
2779         assertNull(calibrator.getEstimatedMyx());
2780         assertNull(calibrator.getEstimatedMyz());
2781         assertNull(calibrator.getEstimatedMzx());
2782         assertNull(calibrator.getEstimatedMzy());
2783         assertNull(calibrator.getEstimatedCovariance());
2784         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2785         assertNull(calibrator.getGroundTruthGravityNorm());
2786         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2787         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2788     }
2789 
2790     @Test
2791     public void testConstructor27() throws WrongSizeException {
2792         final Collection<StandardDeviationBodyKinematics> measurements =
2793                 Collections.emptyList();
2794 
2795         final Matrix ba = generateBa();
2796         final double biasX = ba.getElementAtIndex(0);
2797         final double biasY = ba.getElementAtIndex(1);
2798         final double biasZ = ba.getElementAtIndex(2);
2799 
2800         final Matrix ma = generateMaCommonAxis();
2801         final double sx = ma.getElementAt(0, 0);
2802         final double sy = ma.getElementAt(1, 1);
2803         final double sz = ma.getElementAt(2, 2);
2804 
2805         final KnownPositionAccelerometerCalibrator calibrator =
2806                 new KnownPositionAccelerometerCalibrator(measurements,
2807                         biasX, biasY, biasZ, sx, sy, sz, this);
2808 
2809         // check default values
2810         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2811         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2812         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2813         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2814         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2815         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2816         final Acceleration bx2 = new Acceleration(0.0,
2817                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2818         calibrator.getInitialBiasXAsAcceleration(bx2);
2819         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2820         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2821         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2822         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2823         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2824         final Acceleration by2 = new Acceleration(0.0,
2825                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2826         calibrator.getInitialBiasYAsAcceleration(by2);
2827         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2828         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2829         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2830         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2831         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2832         final Acceleration bz2 = new Acceleration(0.0,
2833                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2834         calibrator.getInitialBiasZAsAcceleration(bz2);
2835         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2836         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2837         assertEquals(calibrator.getInitialSx(), sx, 0.0);
2838         assertEquals(calibrator.getInitialSy(), sy, 0.0);
2839         assertEquals(calibrator.getInitialSz(), sz, 0.0);
2840         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2841         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2842         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2843         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2844         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2845         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2846         final double[] bias1 = calibrator.getInitialBias();
2847         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2848         final double[] bias2 = new double[3];
2849         calibrator.getInitialBias(bias2);
2850         assertArrayEquals(bias1, bias2, 0.0);
2851         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2852         assertEquals(b1, ba);
2853         final Matrix b2 = new Matrix(3, 1);
2854         calibrator.getInitialBiasAsMatrix(b2);
2855         assertEquals(b1, b2);
2856         final Matrix ma1 = calibrator.getInitialMa();
2857         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2858         final Matrix ma2 = new Matrix(3, 3);
2859         calibrator.getInitialMa(ma2);
2860         assertEquals(ma1, ma2);
2861         assertNull(calibrator.getEcefPosition());
2862         assertNull(calibrator.getNedPosition());
2863         assertFalse(calibrator.getNedPosition(null));
2864         assertSame(calibrator.getMeasurements(), measurements);
2865         assertFalse(calibrator.isCommonAxisUsed());
2866         assertSame(calibrator.getListener(), this);
2867         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
2868         assertFalse(calibrator.isReady());
2869         assertFalse(calibrator.isRunning());
2870         assertNull(calibrator.getEstimatedBiases());
2871         assertFalse(calibrator.getEstimatedBiases(null));
2872         assertNull(calibrator.getEstimatedBiasesAsMatrix());
2873         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2874         assertNull(calibrator.getEstimatedBiasFx());
2875         assertNull(calibrator.getEstimatedBiasFy());
2876         assertNull(calibrator.getEstimatedBiasFz());
2877         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2878         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2879         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2880         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2881         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2882         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2883         assertNull(calibrator.getEstimatedMa());
2884         assertNull(calibrator.getEstimatedSx());
2885         assertNull(calibrator.getEstimatedSy());
2886         assertNull(calibrator.getEstimatedSz());
2887         assertNull(calibrator.getEstimatedMxy());
2888         assertNull(calibrator.getEstimatedMxz());
2889         assertNull(calibrator.getEstimatedMyx());
2890         assertNull(calibrator.getEstimatedMyz());
2891         assertNull(calibrator.getEstimatedMzx());
2892         assertNull(calibrator.getEstimatedMzy());
2893         assertNull(calibrator.getEstimatedCovariance());
2894         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2895         assertNull(calibrator.getGroundTruthGravityNorm());
2896         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2897         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2898     }
2899 
2900     @Test
2901     public void testConstructor28() throws WrongSizeException {
2902         final Matrix ba = generateBa();
2903         final double biasX = ba.getElementAtIndex(0);
2904         final double biasY = ba.getElementAtIndex(1);
2905         final double biasZ = ba.getElementAtIndex(2);
2906 
2907         final Matrix ma = generateMaCommonAxis();
2908         final double sx = ma.getElementAt(0, 0);
2909         final double sy = ma.getElementAt(1, 1);
2910         final double sz = ma.getElementAt(2, 2);
2911 
2912         final KnownPositionAccelerometerCalibrator calibrator =
2913                 new KnownPositionAccelerometerCalibrator(true,
2914                         biasX, biasY, biasZ, sx, sy, sz);
2915 
2916         // check default values
2917         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2918         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2919         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2920         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2921         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2922         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2923         final Acceleration bx2 = new Acceleration(0.0,
2924                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2925         calibrator.getInitialBiasXAsAcceleration(bx2);
2926         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2927         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2928         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2929         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2930         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2931         final Acceleration by2 = new Acceleration(0.0,
2932                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2933         calibrator.getInitialBiasYAsAcceleration(by2);
2934         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2935         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2936         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2937         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2938         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2939         final Acceleration bz2 = new Acceleration(0.0,
2940                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2941         calibrator.getInitialBiasZAsAcceleration(bz2);
2942         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2943         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2944         assertEquals(calibrator.getInitialSx(), sx, 0.0);
2945         assertEquals(calibrator.getInitialSy(), sy, 0.0);
2946         assertEquals(calibrator.getInitialSz(), sz, 0.0);
2947         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2948         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2949         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2950         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2951         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2952         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2953         final double[] bias1 = calibrator.getInitialBias();
2954         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2955         final double[] bias2 = new double[3];
2956         calibrator.getInitialBias(bias2);
2957         assertArrayEquals(bias1, bias2, 0.0);
2958         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2959         assertEquals(b1, ba);
2960         final Matrix b2 = new Matrix(3, 1);
2961         calibrator.getInitialBiasAsMatrix(b2);
2962         assertEquals(b1, b2);
2963         final Matrix ma1 = calibrator.getInitialMa();
2964         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2965         final Matrix ma2 = new Matrix(3, 3);
2966         calibrator.getInitialMa(ma2);
2967         assertEquals(ma1, ma2);
2968         assertNull(calibrator.getEcefPosition());
2969         assertNull(calibrator.getNedPosition());
2970         assertFalse(calibrator.getNedPosition(null));
2971         assertNull(calibrator.getMeasurements());
2972         assertTrue(calibrator.isCommonAxisUsed());
2973         assertNull(calibrator.getListener());
2974         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2975         assertFalse(calibrator.isReady());
2976         assertFalse(calibrator.isRunning());
2977         assertNull(calibrator.getEstimatedBiases());
2978         assertFalse(calibrator.getEstimatedBiases(null));
2979         assertNull(calibrator.getEstimatedBiasesAsMatrix());
2980         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2981         assertNull(calibrator.getEstimatedBiasFx());
2982         assertNull(calibrator.getEstimatedBiasFy());
2983         assertNull(calibrator.getEstimatedBiasFz());
2984         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2985         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2986         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2987         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2988         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2989         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2990         assertNull(calibrator.getEstimatedMa());
2991         assertNull(calibrator.getEstimatedSx());
2992         assertNull(calibrator.getEstimatedSy());
2993         assertNull(calibrator.getEstimatedSz());
2994         assertNull(calibrator.getEstimatedMxy());
2995         assertNull(calibrator.getEstimatedMxz());
2996         assertNull(calibrator.getEstimatedMyx());
2997         assertNull(calibrator.getEstimatedMyz());
2998         assertNull(calibrator.getEstimatedMzx());
2999         assertNull(calibrator.getEstimatedMzy());
3000         assertNull(calibrator.getEstimatedCovariance());
3001         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3002         assertNull(calibrator.getGroundTruthGravityNorm());
3003         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3004         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3005     }
3006 
3007     @Test
3008     public void testConstructor29() throws WrongSizeException {
3009         final Matrix ba = generateBa();
3010         final double biasX = ba.getElementAtIndex(0);
3011         final double biasY = ba.getElementAtIndex(1);
3012         final double biasZ = ba.getElementAtIndex(2);
3013 
3014         final Matrix ma = generateMaCommonAxis();
3015         final double sx = ma.getElementAt(0, 0);
3016         final double sy = ma.getElementAt(1, 1);
3017         final double sz = ma.getElementAt(2, 2);
3018 
3019         final KnownPositionAccelerometerCalibrator calibrator =
3020                 new KnownPositionAccelerometerCalibrator(true,
3021                         biasX, biasY, biasZ, sx, sy, sz, this);
3022 
3023         // check default values
3024         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3025         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3026         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3027         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3028         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3029         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3030         final Acceleration bx2 = new Acceleration(0.0,
3031                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3032         calibrator.getInitialBiasXAsAcceleration(bx2);
3033         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3034         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3035         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3036         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3037         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3038         final Acceleration by2 = new Acceleration(0.0,
3039                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3040         calibrator.getInitialBiasYAsAcceleration(by2);
3041         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3042         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3043         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3044         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3045         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3046         final Acceleration bz2 = new Acceleration(0.0,
3047                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3048         calibrator.getInitialBiasZAsAcceleration(bz2);
3049         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3050         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3051         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3052         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3053         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3054         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3055         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3056         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3057         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3058         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3059         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3060         final double[] bias1 = calibrator.getInitialBias();
3061         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3062         final double[] bias2 = new double[3];
3063         calibrator.getInitialBias(bias2);
3064         assertArrayEquals(bias1, bias2, 0.0);
3065         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3066         assertEquals(b1, ba);
3067         final Matrix b2 = new Matrix(3, 1);
3068         calibrator.getInitialBiasAsMatrix(b2);
3069         assertEquals(b1, b2);
3070         final Matrix ma1 = calibrator.getInitialMa();
3071         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3072         final Matrix ma2 = new Matrix(3, 3);
3073         calibrator.getInitialMa(ma2);
3074         assertEquals(ma1, ma2);
3075         assertNull(calibrator.getEcefPosition());
3076         assertNull(calibrator.getNedPosition());
3077         assertFalse(calibrator.getNedPosition(null));
3078         assertNull(calibrator.getMeasurements());
3079         assertTrue(calibrator.isCommonAxisUsed());
3080         assertSame(calibrator.getListener(), this);
3081         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3082         assertFalse(calibrator.isReady());
3083         assertFalse(calibrator.isRunning());
3084         assertNull(calibrator.getEstimatedBiases());
3085         assertFalse(calibrator.getEstimatedBiases(null));
3086         assertNull(calibrator.getEstimatedBiasesAsMatrix());
3087         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3088         assertNull(calibrator.getEstimatedBiasFx());
3089         assertNull(calibrator.getEstimatedBiasFy());
3090         assertNull(calibrator.getEstimatedBiasFz());
3091         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3092         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3093         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3094         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3095         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3096         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3097         assertNull(calibrator.getEstimatedMa());
3098         assertNull(calibrator.getEstimatedSx());
3099         assertNull(calibrator.getEstimatedSy());
3100         assertNull(calibrator.getEstimatedSz());
3101         assertNull(calibrator.getEstimatedMxy());
3102         assertNull(calibrator.getEstimatedMxz());
3103         assertNull(calibrator.getEstimatedMyx());
3104         assertNull(calibrator.getEstimatedMyz());
3105         assertNull(calibrator.getEstimatedMzx());
3106         assertNull(calibrator.getEstimatedMzy());
3107         assertNull(calibrator.getEstimatedCovariance());
3108         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3109         assertNull(calibrator.getGroundTruthGravityNorm());
3110         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3111         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3112     }
3113 
3114     @Test
3115     public void testConstructor30() throws WrongSizeException {
3116         final Collection<StandardDeviationBodyKinematics> measurements =
3117                 Collections.emptyList();
3118         final Matrix ba = generateBa();
3119         final double biasX = ba.getElementAtIndex(0);
3120         final double biasY = ba.getElementAtIndex(1);
3121         final double biasZ = ba.getElementAtIndex(2);
3122 
3123         final Matrix ma = generateMaCommonAxis();
3124         final double sx = ma.getElementAt(0, 0);
3125         final double sy = ma.getElementAt(1, 1);
3126         final double sz = ma.getElementAt(2, 2);
3127 
3128         final KnownPositionAccelerometerCalibrator calibrator =
3129                 new KnownPositionAccelerometerCalibrator(measurements,
3130                         true, biasX, biasY, biasZ, sx, sy, sz);
3131 
3132         // check default values
3133         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3134         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3135         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3136         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3137         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3138         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3139         final Acceleration bx2 = new Acceleration(0.0,
3140                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3141         calibrator.getInitialBiasXAsAcceleration(bx2);
3142         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3143         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3144         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3145         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3146         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3147         final Acceleration by2 = new Acceleration(0.0,
3148                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3149         calibrator.getInitialBiasYAsAcceleration(by2);
3150         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3151         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3152         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3153         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3154         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3155         final Acceleration bz2 = new Acceleration(0.0,
3156                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3157         calibrator.getInitialBiasZAsAcceleration(bz2);
3158         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3159         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3160         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3161         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3162         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3163         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3164         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3165         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3166         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3167         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3168         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3169         final double[] bias1 = calibrator.getInitialBias();
3170         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3171         final double[] bias2 = new double[3];
3172         calibrator.getInitialBias(bias2);
3173         assertArrayEquals(bias1, bias2, 0.0);
3174         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3175         assertEquals(b1, ba);
3176         final Matrix b2 = new Matrix(3, 1);
3177         calibrator.getInitialBiasAsMatrix(b2);
3178         assertEquals(b1, b2);
3179         final Matrix ma1 = calibrator.getInitialMa();
3180         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3181         final Matrix ma2 = new Matrix(3, 3);
3182         calibrator.getInitialMa(ma2);
3183         assertEquals(ma1, ma2);
3184         assertNull(calibrator.getEcefPosition());
3185         assertNull(calibrator.getNedPosition());
3186         assertFalse(calibrator.getNedPosition(null));
3187         assertSame(calibrator.getMeasurements(), measurements);
3188         assertTrue(calibrator.isCommonAxisUsed());
3189         assertNull(calibrator.getListener());
3190         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3191         assertFalse(calibrator.isReady());
3192         assertFalse(calibrator.isRunning());
3193         assertNull(calibrator.getEstimatedBiases());
3194         assertFalse(calibrator.getEstimatedBiases(null));
3195         assertNull(calibrator.getEstimatedBiasesAsMatrix());
3196         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3197         assertNull(calibrator.getEstimatedBiasFx());
3198         assertNull(calibrator.getEstimatedBiasFy());
3199         assertNull(calibrator.getEstimatedBiasFz());
3200         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3201         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3202         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3203         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3204         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3205         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3206         assertNull(calibrator.getEstimatedMa());
3207         assertNull(calibrator.getEstimatedSx());
3208         assertNull(calibrator.getEstimatedSy());
3209         assertNull(calibrator.getEstimatedSz());
3210         assertNull(calibrator.getEstimatedMxy());
3211         assertNull(calibrator.getEstimatedMxz());
3212         assertNull(calibrator.getEstimatedMyx());
3213         assertNull(calibrator.getEstimatedMyz());
3214         assertNull(calibrator.getEstimatedMzx());
3215         assertNull(calibrator.getEstimatedMzy());
3216         assertNull(calibrator.getEstimatedCovariance());
3217         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3218         assertNull(calibrator.getGroundTruthGravityNorm());
3219         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3220         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3221     }
3222 
3223     @Test
3224     public void testConstructor31() throws WrongSizeException {
3225         final Collection<StandardDeviationBodyKinematics> measurements =
3226                 Collections.emptyList();
3227         final Matrix ba = generateBa();
3228         final double biasX = ba.getElementAtIndex(0);
3229         final double biasY = ba.getElementAtIndex(1);
3230         final double biasZ = ba.getElementAtIndex(2);
3231 
3232         final Matrix ma = generateMaCommonAxis();
3233         final double sx = ma.getElementAt(0, 0);
3234         final double sy = ma.getElementAt(1, 1);
3235         final double sz = ma.getElementAt(2, 2);
3236 
3237         final KnownPositionAccelerometerCalibrator calibrator =
3238                 new KnownPositionAccelerometerCalibrator(measurements,
3239                         true, biasX, biasY, biasZ, sx, sy, sz,
3240                         this);
3241 
3242         // check default values
3243         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3244         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3245         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3246         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3247         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3248         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3249         final Acceleration bx2 = new Acceleration(0.0,
3250                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3251         calibrator.getInitialBiasXAsAcceleration(bx2);
3252         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3253         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3254         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3255         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3256         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3257         final Acceleration by2 = new Acceleration(0.0,
3258                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3259         calibrator.getInitialBiasYAsAcceleration(by2);
3260         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3261         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3262         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3263         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3264         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3265         final Acceleration bz2 = new Acceleration(0.0,
3266                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3267         calibrator.getInitialBiasZAsAcceleration(bz2);
3268         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3269         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3270         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3271         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3272         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3273         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3274         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3275         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3276         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3277         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3278         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3279         final double[] bias1 = calibrator.getInitialBias();
3280         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3281         final double[] bias2 = new double[3];
3282         calibrator.getInitialBias(bias2);
3283         assertArrayEquals(bias1, bias2, 0.0);
3284         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3285         assertEquals(b1, ba);
3286         final Matrix b2 = new Matrix(3, 1);
3287         calibrator.getInitialBiasAsMatrix(b2);
3288         assertEquals(b1, b2);
3289         final Matrix ma1 = calibrator.getInitialMa();
3290         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3291         final Matrix ma2 = new Matrix(3, 3);
3292         calibrator.getInitialMa(ma2);
3293         assertEquals(ma1, ma2);
3294         assertNull(calibrator.getEcefPosition());
3295         assertNull(calibrator.getNedPosition());
3296         assertFalse(calibrator.getNedPosition(null));
3297         assertSame(calibrator.getMeasurements(), measurements);
3298         assertTrue(calibrator.isCommonAxisUsed());
3299         assertSame(calibrator.getListener(), this);
3300         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3301         assertFalse(calibrator.isReady());
3302         assertFalse(calibrator.isRunning());
3303         assertNull(calibrator.getEstimatedBiases());
3304         assertFalse(calibrator.getEstimatedBiases(null));
3305         assertNull(calibrator.getEstimatedBiasesAsMatrix());
3306         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3307         assertNull(calibrator.getEstimatedBiasFx());
3308         assertNull(calibrator.getEstimatedBiasFy());
3309         assertNull(calibrator.getEstimatedBiasFz());
3310         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3311         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3312         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3313         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3314         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3315         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3316         assertNull(calibrator.getEstimatedMa());
3317         assertNull(calibrator.getEstimatedSx());
3318         assertNull(calibrator.getEstimatedSy());
3319         assertNull(calibrator.getEstimatedSz());
3320         assertNull(calibrator.getEstimatedMxy());
3321         assertNull(calibrator.getEstimatedMxz());
3322         assertNull(calibrator.getEstimatedMyx());
3323         assertNull(calibrator.getEstimatedMyz());
3324         assertNull(calibrator.getEstimatedMzx());
3325         assertNull(calibrator.getEstimatedMzy());
3326         assertNull(calibrator.getEstimatedCovariance());
3327         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3328         assertNull(calibrator.getGroundTruthGravityNorm());
3329         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3330         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3331     }
3332 
3333     @Test
3334     public void testConstructor32() throws WrongSizeException {
3335         final Matrix ba = generateBa();
3336         final double biasX = ba.getElementAtIndex(0);
3337         final double biasY = ba.getElementAtIndex(1);
3338         final double biasZ = ba.getElementAtIndex(2);
3339 
3340         final Matrix ma = generateMaCommonAxis();
3341         final double sx = ma.getElementAt(0, 0);
3342         final double sy = ma.getElementAt(1, 1);
3343         final double sz = ma.getElementAt(2, 2);
3344 
3345         final Acceleration bx = new Acceleration(biasX,
3346                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3347         final Acceleration by = new Acceleration(biasY,
3348                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3349         final Acceleration bz = new Acceleration(biasZ,
3350                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3351 
3352         final KnownPositionAccelerometerCalibrator calibrator =
3353                 new KnownPositionAccelerometerCalibrator(bx, by, bz,
3354                         sx, sy, sz);
3355 
3356         // check default values
3357         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3358         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3359         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3360         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3361         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3362         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3363         final Acceleration bx2 = new Acceleration(0.0,
3364                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3365         calibrator.getInitialBiasXAsAcceleration(bx2);
3366         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3367         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3368         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3369         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3370         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3371         final Acceleration by2 = new Acceleration(0.0,
3372                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3373         calibrator.getInitialBiasYAsAcceleration(by2);
3374         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3375         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3376         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3377         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3378         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3379         final Acceleration bz2 = new Acceleration(0.0,
3380                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3381         calibrator.getInitialBiasZAsAcceleration(bz2);
3382         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3383         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3384         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3385         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3386         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3387         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3388         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3389         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3390         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3391         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3392         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3393         final double[] bias1 = calibrator.getInitialBias();
3394         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3395         final double[] bias2 = new double[3];
3396         calibrator.getInitialBias(bias2);
3397         assertArrayEquals(bias1, bias2, 0.0);
3398         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3399         assertEquals(b1, ba);
3400         final Matrix b2 = new Matrix(3, 1);
3401         calibrator.getInitialBiasAsMatrix(b2);
3402         assertEquals(b1, b2);
3403         final Matrix ma1 = calibrator.getInitialMa();
3404         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3405         final Matrix ma2 = new Matrix(3, 3);
3406         calibrator.getInitialMa(ma2);
3407         assertEquals(ma1, ma2);
3408         assertNull(calibrator.getEcefPosition());
3409         assertNull(calibrator.getNedPosition());
3410         assertFalse(calibrator.getNedPosition(null));
3411         assertNull(calibrator.getMeasurements());
3412         assertFalse(calibrator.isCommonAxisUsed());
3413         assertNull(calibrator.getListener());
3414         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
3415         assertFalse(calibrator.isReady());
3416         assertFalse(calibrator.isRunning());
3417         assertNull(calibrator.getEstimatedBiases());
3418         assertFalse(calibrator.getEstimatedBiases(null));
3419         assertNull(calibrator.getEstimatedBiasesAsMatrix());
3420         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3421         assertNull(calibrator.getEstimatedBiasFx());
3422         assertNull(calibrator.getEstimatedBiasFy());
3423         assertNull(calibrator.getEstimatedBiasFz());
3424         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3425         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3426         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3427         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3428         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3429         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3430         assertNull(calibrator.getEstimatedMa());
3431         assertNull(calibrator.getEstimatedSx());
3432         assertNull(calibrator.getEstimatedSy());
3433         assertNull(calibrator.getEstimatedSz());
3434         assertNull(calibrator.getEstimatedMxy());
3435         assertNull(calibrator.getEstimatedMxz());
3436         assertNull(calibrator.getEstimatedMyx());
3437         assertNull(calibrator.getEstimatedMyz());
3438         assertNull(calibrator.getEstimatedMzx());
3439         assertNull(calibrator.getEstimatedMzy());
3440         assertNull(calibrator.getEstimatedCovariance());
3441         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3442         assertNull(calibrator.getGroundTruthGravityNorm());
3443         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3444         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3445     }
3446 
3447     @Test
3448     public void testConstructor33() throws WrongSizeException {
3449         final Matrix ba = generateBa();
3450         final double biasX = ba.getElementAtIndex(0);
3451         final double biasY = ba.getElementAtIndex(1);
3452         final double biasZ = ba.getElementAtIndex(2);
3453 
3454         final Matrix ma = generateMaCommonAxis();
3455         final double sx = ma.getElementAt(0, 0);
3456         final double sy = ma.getElementAt(1, 1);
3457         final double sz = ma.getElementAt(2, 2);
3458 
3459         final Acceleration bx = new Acceleration(biasX,
3460                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3461         final Acceleration by = new Acceleration(biasY,
3462                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3463         final Acceleration bz = new Acceleration(biasZ,
3464                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3465 
3466         final KnownPositionAccelerometerCalibrator calibrator =
3467                 new KnownPositionAccelerometerCalibrator(bx, by, bz,
3468                         sx, sy, sz, this);
3469 
3470         // check default values
3471         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3472         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3473         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3474         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3475         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3476         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3477         final Acceleration bx2 = new Acceleration(0.0,
3478                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3479         calibrator.getInitialBiasXAsAcceleration(bx2);
3480         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3481         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3482         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3483         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3484         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3485         final Acceleration by2 = new Acceleration(0.0,
3486                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3487         calibrator.getInitialBiasYAsAcceleration(by2);
3488         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3489         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3490         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3491         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3492         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3493         final Acceleration bz2 = new Acceleration(0.0,
3494                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3495         calibrator.getInitialBiasZAsAcceleration(bz2);
3496         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3497         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3498         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3499         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3500         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3501         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3502         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3503         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3504         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3505         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3506         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3507         final double[] bias1 = calibrator.getInitialBias();
3508         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3509         final double[] bias2 = new double[3];
3510         calibrator.getInitialBias(bias2);
3511         assertArrayEquals(bias1, bias2, 0.0);
3512         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3513         assertEquals(b1, ba);
3514         final Matrix b2 = new Matrix(3, 1);
3515         calibrator.getInitialBiasAsMatrix(b2);
3516         assertEquals(b1, b2);
3517         final Matrix ma1 = calibrator.getInitialMa();
3518         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3519         final Matrix ma2 = new Matrix(3, 3);
3520         calibrator.getInitialMa(ma2);
3521         assertEquals(ma1, ma2);
3522         assertNull(calibrator.getEcefPosition());
3523         assertNull(calibrator.getNedPosition());
3524         assertFalse(calibrator.getNedPosition(null));
3525         assertNull(calibrator.getMeasurements());
3526         assertFalse(calibrator.isCommonAxisUsed());
3527         assertSame(calibrator.getListener(), this);
3528         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
3529         assertFalse(calibrator.isReady());
3530         assertFalse(calibrator.isRunning());
3531         assertNull(calibrator.getEstimatedBiases());
3532         assertFalse(calibrator.getEstimatedBiases(null));
3533         assertNull(calibrator.getEstimatedBiasesAsMatrix());
3534         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3535         assertNull(calibrator.getEstimatedBiasFx());
3536         assertNull(calibrator.getEstimatedBiasFy());
3537         assertNull(calibrator.getEstimatedBiasFz());
3538         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3539         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3540         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3541         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3542         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3543         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3544         assertNull(calibrator.getEstimatedMa());
3545         assertNull(calibrator.getEstimatedSx());
3546         assertNull(calibrator.getEstimatedSy());
3547         assertNull(calibrator.getEstimatedSz());
3548         assertNull(calibrator.getEstimatedMxy());
3549         assertNull(calibrator.getEstimatedMxz());
3550         assertNull(calibrator.getEstimatedMyx());
3551         assertNull(calibrator.getEstimatedMyz());
3552         assertNull(calibrator.getEstimatedMzx());
3553         assertNull(calibrator.getEstimatedMzy());
3554         assertNull(calibrator.getEstimatedCovariance());
3555         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3556         assertNull(calibrator.getGroundTruthGravityNorm());
3557         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3558         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3559     }
3560 
3561     @Test
3562     public void testConstructor34() throws WrongSizeException {
3563         final Collection<StandardDeviationBodyKinematics> measurements =
3564                 Collections.emptyList();
3565 
3566         final Matrix ba = generateBa();
3567         final double biasX = ba.getElementAtIndex(0);
3568         final double biasY = ba.getElementAtIndex(1);
3569         final double biasZ = ba.getElementAtIndex(2);
3570 
3571         final Matrix ma = generateMaCommonAxis();
3572         final double sx = ma.getElementAt(0, 0);
3573         final double sy = ma.getElementAt(1, 1);
3574         final double sz = ma.getElementAt(2, 2);
3575 
3576         final Acceleration bx = new Acceleration(biasX,
3577                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3578         final Acceleration by = new Acceleration(biasY,
3579                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3580         final Acceleration bz = new Acceleration(biasZ,
3581                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3582 
3583         final KnownPositionAccelerometerCalibrator calibrator =
3584                 new KnownPositionAccelerometerCalibrator(measurements,
3585                         bx, by, bz, sx, sy, sz);
3586 
3587         // check default values
3588         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3589         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3590         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3591         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3592         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3593         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3594         final Acceleration bx2 = new Acceleration(0.0,
3595                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3596         calibrator.getInitialBiasXAsAcceleration(bx2);
3597         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3598         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3599         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3600         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3601         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3602         final Acceleration by2 = new Acceleration(0.0,
3603                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3604         calibrator.getInitialBiasYAsAcceleration(by2);
3605         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3606         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3607         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3608         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3609         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3610         final Acceleration bz2 = new Acceleration(0.0,
3611                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3612         calibrator.getInitialBiasZAsAcceleration(bz2);
3613         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3614         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3615         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3616         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3617         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3618         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3619         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3620         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3621         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3622         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3623         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3624         final double[] bias1 = calibrator.getInitialBias();
3625         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3626         final double[] bias2 = new double[3];
3627         calibrator.getInitialBias(bias2);
3628         assertArrayEquals(bias1, bias2, 0.0);
3629         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3630         assertEquals(b1, ba);
3631         final Matrix b2 = new Matrix(3, 1);
3632         calibrator.getInitialBiasAsMatrix(b2);
3633         assertEquals(b1, b2);
3634         final Matrix ma1 = calibrator.getInitialMa();
3635         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3636         final Matrix ma2 = new Matrix(3, 3);
3637         calibrator.getInitialMa(ma2);
3638         assertEquals(ma1, ma2);
3639         assertNull(calibrator.getEcefPosition());
3640         assertNull(calibrator.getNedPosition());
3641         assertFalse(calibrator.getNedPosition(null));
3642         assertSame(calibrator.getMeasurements(), measurements);
3643         assertFalse(calibrator.isCommonAxisUsed());
3644         assertNull(calibrator.getListener());
3645         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
3646         assertFalse(calibrator.isReady());
3647         assertFalse(calibrator.isRunning());
3648         assertNull(calibrator.getEstimatedBiases());
3649         assertFalse(calibrator.getEstimatedBiases(null));
3650         assertNull(calibrator.getEstimatedBiasesAsMatrix());
3651         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3652         assertNull(calibrator.getEstimatedBiasFx());
3653         assertNull(calibrator.getEstimatedBiasFy());
3654         assertNull(calibrator.getEstimatedBiasFz());
3655         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3656         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3657         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3658         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3659         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3660         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3661         assertNull(calibrator.getEstimatedMa());
3662         assertNull(calibrator.getEstimatedSx());
3663         assertNull(calibrator.getEstimatedSy());
3664         assertNull(calibrator.getEstimatedSz());
3665         assertNull(calibrator.getEstimatedMxy());
3666         assertNull(calibrator.getEstimatedMxz());
3667         assertNull(calibrator.getEstimatedMyx());
3668         assertNull(calibrator.getEstimatedMyz());
3669         assertNull(calibrator.getEstimatedMzx());
3670         assertNull(calibrator.getEstimatedMzy());
3671         assertNull(calibrator.getEstimatedCovariance());
3672         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3673         assertNull(calibrator.getGroundTruthGravityNorm());
3674         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3675         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3676     }
3677 
3678     @Test
3679     public void testConstructor35() throws WrongSizeException {
3680         final Collection<StandardDeviationBodyKinematics> measurements =
3681                 Collections.emptyList();
3682 
3683         final Matrix ba = generateBa();
3684         final double biasX = ba.getElementAtIndex(0);
3685         final double biasY = ba.getElementAtIndex(1);
3686         final double biasZ = ba.getElementAtIndex(2);
3687 
3688         final Matrix ma = generateMaCommonAxis();
3689         final double sx = ma.getElementAt(0, 0);
3690         final double sy = ma.getElementAt(1, 1);
3691         final double sz = ma.getElementAt(2, 2);
3692 
3693         final Acceleration bx = new Acceleration(biasX,
3694                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3695         final Acceleration by = new Acceleration(biasY,
3696                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3697         final Acceleration bz = new Acceleration(biasZ,
3698                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3699 
3700         final KnownPositionAccelerometerCalibrator calibrator =
3701                 new KnownPositionAccelerometerCalibrator(measurements,
3702                         bx, by, bz, sx, sy, sz, this);
3703 
3704         // check default values
3705         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3706         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3707         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3708         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3709         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3710         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3711         final Acceleration bx2 = new Acceleration(0.0,
3712                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3713         calibrator.getInitialBiasXAsAcceleration(bx2);
3714         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3715         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3716         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3717         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3718         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3719         final Acceleration by2 = new Acceleration(0.0,
3720                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3721         calibrator.getInitialBiasYAsAcceleration(by2);
3722         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3723         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3724         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3725         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3726         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3727         final Acceleration bz2 = new Acceleration(0.0,
3728                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3729         calibrator.getInitialBiasZAsAcceleration(bz2);
3730         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3731         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3732         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3733         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3734         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3735         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3736         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3737         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3738         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3739         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3740         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3741         final double[] bias1 = calibrator.getInitialBias();
3742         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3743         final double[] bias2 = new double[3];
3744         calibrator.getInitialBias(bias2);
3745         assertArrayEquals(bias1, bias2, 0.0);
3746         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3747         assertEquals(b1, ba);
3748         final Matrix b2 = new Matrix(3, 1);
3749         calibrator.getInitialBiasAsMatrix(b2);
3750         assertEquals(b1, b2);
3751         final Matrix ma1 = calibrator.getInitialMa();
3752         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3753         final Matrix ma2 = new Matrix(3, 3);
3754         calibrator.getInitialMa(ma2);
3755         assertEquals(ma1, ma2);
3756         assertNull(calibrator.getEcefPosition());
3757         assertNull(calibrator.getNedPosition());
3758         assertFalse(calibrator.getNedPosition(null));
3759         assertSame(calibrator.getMeasurements(), measurements);
3760         assertFalse(calibrator.isCommonAxisUsed());
3761         assertSame(calibrator.getListener(), this);
3762         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
3763         assertFalse(calibrator.isReady());
3764         assertFalse(calibrator.isRunning());
3765         assertNull(calibrator.getEstimatedBiases());
3766         assertFalse(calibrator.getEstimatedBiases(null));
3767         assertNull(calibrator.getEstimatedBiasesAsMatrix());
3768         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3769         assertNull(calibrator.getEstimatedBiasFx());
3770         assertNull(calibrator.getEstimatedBiasFy());
3771         assertNull(calibrator.getEstimatedBiasFz());
3772         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3773         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3774         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3775         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3776         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3777         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3778         assertNull(calibrator.getEstimatedMa());
3779         assertNull(calibrator.getEstimatedSx());
3780         assertNull(calibrator.getEstimatedSy());
3781         assertNull(calibrator.getEstimatedSz());
3782         assertNull(calibrator.getEstimatedMxy());
3783         assertNull(calibrator.getEstimatedMxz());
3784         assertNull(calibrator.getEstimatedMyx());
3785         assertNull(calibrator.getEstimatedMyz());
3786         assertNull(calibrator.getEstimatedMzx());
3787         assertNull(calibrator.getEstimatedMzy());
3788         assertNull(calibrator.getEstimatedCovariance());
3789         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3790         assertNull(calibrator.getGroundTruthGravityNorm());
3791         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3792         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3793     }
3794 
3795     @Test
3796     public void testConstructor36() throws WrongSizeException {
3797         final Matrix ba = generateBa();
3798         final double biasX = ba.getElementAtIndex(0);
3799         final double biasY = ba.getElementAtIndex(1);
3800         final double biasZ = ba.getElementAtIndex(2);
3801 
3802         final Matrix ma = generateMaCommonAxis();
3803         final double sx = ma.getElementAt(0, 0);
3804         final double sy = ma.getElementAt(1, 1);
3805         final double sz = ma.getElementAt(2, 2);
3806 
3807         final Acceleration bx = new Acceleration(biasX,
3808                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3809         final Acceleration by = new Acceleration(biasY,
3810                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3811         final Acceleration bz = new Acceleration(biasZ,
3812                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3813 
3814         final KnownPositionAccelerometerCalibrator calibrator =
3815                 new KnownPositionAccelerometerCalibrator(true,
3816                         bx, by, bz, sx, sy, sz);
3817 
3818         // check default values
3819         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3820         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3821         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3822         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3823         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3824         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3825         final Acceleration bx2 = new Acceleration(0.0,
3826                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3827         calibrator.getInitialBiasXAsAcceleration(bx2);
3828         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3829         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3830         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3831         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3832         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3833         final Acceleration by2 = new Acceleration(0.0,
3834                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3835         calibrator.getInitialBiasYAsAcceleration(by2);
3836         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3837         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3838         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3839         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3840         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3841         final Acceleration bz2 = new Acceleration(0.0,
3842                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3843         calibrator.getInitialBiasZAsAcceleration(bz2);
3844         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3845         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3846         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3847         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3848         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3849         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3850         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3851         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3852         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3853         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3854         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3855         final double[] bias1 = calibrator.getInitialBias();
3856         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3857         final double[] bias2 = new double[3];
3858         calibrator.getInitialBias(bias2);
3859         assertArrayEquals(bias1, bias2, 0.0);
3860         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3861         assertEquals(b1, ba);
3862         final Matrix b2 = new Matrix(3, 1);
3863         calibrator.getInitialBiasAsMatrix(b2);
3864         assertEquals(b1, b2);
3865         final Matrix ma1 = calibrator.getInitialMa();
3866         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3867         final Matrix ma2 = new Matrix(3, 3);
3868         calibrator.getInitialMa(ma2);
3869         assertEquals(ma1, ma2);
3870         assertNull(calibrator.getEcefPosition());
3871         assertNull(calibrator.getNedPosition());
3872         assertFalse(calibrator.getNedPosition(null));
3873         assertNull(calibrator.getMeasurements());
3874         assertTrue(calibrator.isCommonAxisUsed());
3875         assertNull(calibrator.getListener());
3876         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3877         assertFalse(calibrator.isReady());
3878         assertFalse(calibrator.isRunning());
3879         assertNull(calibrator.getEstimatedBiases());
3880         assertFalse(calibrator.getEstimatedBiases(null));
3881         assertNull(calibrator.getEstimatedBiasesAsMatrix());
3882         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3883         assertNull(calibrator.getEstimatedBiasFx());
3884         assertNull(calibrator.getEstimatedBiasFy());
3885         assertNull(calibrator.getEstimatedBiasFz());
3886         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3887         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3888         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3889         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3890         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3891         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3892         assertNull(calibrator.getEstimatedMa());
3893         assertNull(calibrator.getEstimatedSx());
3894         assertNull(calibrator.getEstimatedSy());
3895         assertNull(calibrator.getEstimatedSz());
3896         assertNull(calibrator.getEstimatedMxy());
3897         assertNull(calibrator.getEstimatedMxz());
3898         assertNull(calibrator.getEstimatedMyx());
3899         assertNull(calibrator.getEstimatedMyz());
3900         assertNull(calibrator.getEstimatedMzx());
3901         assertNull(calibrator.getEstimatedMzy());
3902         assertNull(calibrator.getEstimatedCovariance());
3903         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3904         assertNull(calibrator.getGroundTruthGravityNorm());
3905         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3906         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3907     }
3908 
3909     @Test
3910     public void testConstructor37() throws WrongSizeException {
3911         final Matrix ba = generateBa();
3912         final double biasX = ba.getElementAtIndex(0);
3913         final double biasY = ba.getElementAtIndex(1);
3914         final double biasZ = ba.getElementAtIndex(2);
3915 
3916         final Matrix ma = generateMaCommonAxis();
3917         final double sx = ma.getElementAt(0, 0);
3918         final double sy = ma.getElementAt(1, 1);
3919         final double sz = ma.getElementAt(2, 2);
3920 
3921         final Acceleration bx = new Acceleration(biasX,
3922                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3923         final Acceleration by = new Acceleration(biasY,
3924                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3925         final Acceleration bz = new Acceleration(biasZ,
3926                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3927 
3928         final KnownPositionAccelerometerCalibrator calibrator =
3929                 new KnownPositionAccelerometerCalibrator(true,
3930                         bx, by, bz, sx, sy, sz, this);
3931 
3932         // check default values
3933         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3934         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3935         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3936         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3937         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3938         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3939         final Acceleration bx2 = new Acceleration(0.0,
3940                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3941         calibrator.getInitialBiasXAsAcceleration(bx2);
3942         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3943         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3944         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3945         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3946         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3947         final Acceleration by2 = new Acceleration(0.0,
3948                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3949         calibrator.getInitialBiasYAsAcceleration(by2);
3950         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3951         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3952         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3953         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3954         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3955         final Acceleration bz2 = new Acceleration(0.0,
3956                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3957         calibrator.getInitialBiasZAsAcceleration(bz2);
3958         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3959         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3960         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3961         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3962         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3963         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3964         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3965         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3966         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3967         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3968         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3969         final double[] bias1 = calibrator.getInitialBias();
3970         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3971         final double[] bias2 = new double[3];
3972         calibrator.getInitialBias(bias2);
3973         assertArrayEquals(bias1, bias2, 0.0);
3974         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3975         assertEquals(b1, ba);
3976         final Matrix b2 = new Matrix(3, 1);
3977         calibrator.getInitialBiasAsMatrix(b2);
3978         assertEquals(b1, b2);
3979         final Matrix ma1 = calibrator.getInitialMa();
3980         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3981         final Matrix ma2 = new Matrix(3, 3);
3982         calibrator.getInitialMa(ma2);
3983         assertEquals(ma1, ma2);
3984         assertNull(calibrator.getEcefPosition());
3985         assertNull(calibrator.getNedPosition());
3986         assertFalse(calibrator.getNedPosition(null));
3987         assertNull(calibrator.getMeasurements());
3988         assertTrue(calibrator.isCommonAxisUsed());
3989         assertSame(calibrator.getListener(), this);
3990         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3991         assertFalse(calibrator.isReady());
3992         assertFalse(calibrator.isRunning());
3993         assertNull(calibrator.getEstimatedBiases());
3994         assertFalse(calibrator.getEstimatedBiases(null));
3995         assertNull(calibrator.getEstimatedBiasesAsMatrix());
3996         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3997         assertNull(calibrator.getEstimatedBiasFx());
3998         assertNull(calibrator.getEstimatedBiasFy());
3999         assertNull(calibrator.getEstimatedBiasFz());
4000         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4001         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4002         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4003         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4004         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4005         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4006         assertNull(calibrator.getEstimatedMa());
4007         assertNull(calibrator.getEstimatedSx());
4008         assertNull(calibrator.getEstimatedSy());
4009         assertNull(calibrator.getEstimatedSz());
4010         assertNull(calibrator.getEstimatedMxy());
4011         assertNull(calibrator.getEstimatedMxz());
4012         assertNull(calibrator.getEstimatedMyx());
4013         assertNull(calibrator.getEstimatedMyz());
4014         assertNull(calibrator.getEstimatedMzx());
4015         assertNull(calibrator.getEstimatedMzy());
4016         assertNull(calibrator.getEstimatedCovariance());
4017         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4018         assertNull(calibrator.getGroundTruthGravityNorm());
4019         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4020         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4021     }
4022 
4023     @Test
4024     public void testConstructor38() throws WrongSizeException {
4025         final Collection<StandardDeviationBodyKinematics> measurements =
4026                 Collections.emptyList();
4027 
4028         final Matrix ba = generateBa();
4029         final double biasX = ba.getElementAtIndex(0);
4030         final double biasY = ba.getElementAtIndex(1);
4031         final double biasZ = ba.getElementAtIndex(2);
4032 
4033         final Matrix ma = generateMaCommonAxis();
4034         final double sx = ma.getElementAt(0, 0);
4035         final double sy = ma.getElementAt(1, 1);
4036         final double sz = ma.getElementAt(2, 2);
4037 
4038         final Acceleration bx = new Acceleration(biasX,
4039                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4040         final Acceleration by = new Acceleration(biasY,
4041                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4042         final Acceleration bz = new Acceleration(biasZ,
4043                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4044 
4045         final KnownPositionAccelerometerCalibrator calibrator =
4046                 new KnownPositionAccelerometerCalibrator(measurements,
4047                         true, bx, by, bz, sx, sy, sz);
4048 
4049         // check default values
4050         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4051         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4052         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4053         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4054         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4055         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4056         final Acceleration bx2 = new Acceleration(0.0,
4057                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4058         calibrator.getInitialBiasXAsAcceleration(bx2);
4059         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4060         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4061         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4062         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4063         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4064         final Acceleration by2 = new Acceleration(0.0,
4065                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4066         calibrator.getInitialBiasYAsAcceleration(by2);
4067         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4068         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4069         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4070         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4071         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4072         final Acceleration bz2 = new Acceleration(0.0,
4073                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4074         calibrator.getInitialBiasZAsAcceleration(bz2);
4075         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4076         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4077         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4078         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4079         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4080         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
4081         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
4082         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
4083         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
4084         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
4085         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
4086         final double[] bias1 = calibrator.getInitialBias();
4087         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4088         final double[] bias2 = new double[3];
4089         calibrator.getInitialBias(bias2);
4090         assertArrayEquals(bias1, bias2, 0.0);
4091         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4092         assertEquals(b1, ba);
4093         final Matrix b2 = new Matrix(3, 1);
4094         calibrator.getInitialBiasAsMatrix(b2);
4095         assertEquals(b1, b2);
4096         final Matrix ma1 = calibrator.getInitialMa();
4097         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
4098         final Matrix ma2 = new Matrix(3, 3);
4099         calibrator.getInitialMa(ma2);
4100         assertEquals(ma1, ma2);
4101         assertNull(calibrator.getEcefPosition());
4102         assertNull(calibrator.getNedPosition());
4103         assertFalse(calibrator.getNedPosition(null));
4104         assertSame(calibrator.getMeasurements(), measurements);
4105         assertTrue(calibrator.isCommonAxisUsed());
4106         assertNull(calibrator.getListener());
4107         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4108         assertFalse(calibrator.isReady());
4109         assertFalse(calibrator.isRunning());
4110         assertNull(calibrator.getEstimatedBiases());
4111         assertFalse(calibrator.getEstimatedBiases(null));
4112         assertNull(calibrator.getEstimatedBiasesAsMatrix());
4113         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4114         assertNull(calibrator.getEstimatedBiasFx());
4115         assertNull(calibrator.getEstimatedBiasFy());
4116         assertNull(calibrator.getEstimatedBiasFz());
4117         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4118         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4119         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4120         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4121         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4122         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4123         assertNull(calibrator.getEstimatedMa());
4124         assertNull(calibrator.getEstimatedSx());
4125         assertNull(calibrator.getEstimatedSy());
4126         assertNull(calibrator.getEstimatedSz());
4127         assertNull(calibrator.getEstimatedMxy());
4128         assertNull(calibrator.getEstimatedMxz());
4129         assertNull(calibrator.getEstimatedMyx());
4130         assertNull(calibrator.getEstimatedMyz());
4131         assertNull(calibrator.getEstimatedMzx());
4132         assertNull(calibrator.getEstimatedMzy());
4133         assertNull(calibrator.getEstimatedCovariance());
4134         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4135         assertNull(calibrator.getGroundTruthGravityNorm());
4136         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4137         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4138     }
4139 
4140     @Test
4141     public void testConstructor39() throws WrongSizeException {
4142         final Collection<StandardDeviationBodyKinematics> measurements =
4143                 Collections.emptyList();
4144 
4145         final Matrix ba = generateBa();
4146         final double biasX = ba.getElementAtIndex(0);
4147         final double biasY = ba.getElementAtIndex(1);
4148         final double biasZ = ba.getElementAtIndex(2);
4149 
4150         final Matrix ma = generateMaCommonAxis();
4151         final double sx = ma.getElementAt(0, 0);
4152         final double sy = ma.getElementAt(1, 1);
4153         final double sz = ma.getElementAt(2, 2);
4154 
4155         final Acceleration bx = new Acceleration(biasX,
4156                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4157         final Acceleration by = new Acceleration(biasY,
4158                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4159         final Acceleration bz = new Acceleration(biasZ,
4160                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4161 
4162         final KnownPositionAccelerometerCalibrator calibrator =
4163                 new KnownPositionAccelerometerCalibrator(measurements,
4164                         true, bx, by, bz, sx, sy, sz,
4165                         this);
4166 
4167         // check default values
4168         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4169         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4170         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4171         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4172         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4173         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4174         final Acceleration bx2 = new Acceleration(0.0,
4175                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4176         calibrator.getInitialBiasXAsAcceleration(bx2);
4177         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4178         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4179         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4180         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4181         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4182         final Acceleration by2 = new Acceleration(0.0,
4183                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4184         calibrator.getInitialBiasYAsAcceleration(by2);
4185         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4186         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4187         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4188         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4189         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4190         final Acceleration bz2 = new Acceleration(0.0,
4191                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4192         calibrator.getInitialBiasZAsAcceleration(bz2);
4193         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4194         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4195         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4196         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4197         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4198         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
4199         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
4200         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
4201         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
4202         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
4203         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
4204         final double[] bias1 = calibrator.getInitialBias();
4205         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4206         final double[] bias2 = new double[3];
4207         calibrator.getInitialBias(bias2);
4208         assertArrayEquals(bias1, bias2, 0.0);
4209         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4210         assertEquals(b1, ba);
4211         final Matrix b2 = new Matrix(3, 1);
4212         calibrator.getInitialBiasAsMatrix(b2);
4213         assertEquals(b1, b2);
4214         final Matrix ma1 = calibrator.getInitialMa();
4215         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
4216         final Matrix ma2 = new Matrix(3, 3);
4217         calibrator.getInitialMa(ma2);
4218         assertEquals(ma1, ma2);
4219         assertNull(calibrator.getEcefPosition());
4220         assertNull(calibrator.getNedPosition());
4221         assertFalse(calibrator.getNedPosition(null));
4222         assertSame(calibrator.getMeasurements(), measurements);
4223         assertTrue(calibrator.isCommonAxisUsed());
4224         assertSame(calibrator.getListener(), this);
4225         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4226         assertFalse(calibrator.isReady());
4227         assertFalse(calibrator.isRunning());
4228         assertNull(calibrator.getEstimatedBiases());
4229         assertFalse(calibrator.getEstimatedBiases(null));
4230         assertNull(calibrator.getEstimatedBiasesAsMatrix());
4231         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4232         assertNull(calibrator.getEstimatedBiasFx());
4233         assertNull(calibrator.getEstimatedBiasFy());
4234         assertNull(calibrator.getEstimatedBiasFz());
4235         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4236         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4237         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4238         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4239         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4240         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4241         assertNull(calibrator.getEstimatedMa());
4242         assertNull(calibrator.getEstimatedSx());
4243         assertNull(calibrator.getEstimatedSy());
4244         assertNull(calibrator.getEstimatedSz());
4245         assertNull(calibrator.getEstimatedMxy());
4246         assertNull(calibrator.getEstimatedMxz());
4247         assertNull(calibrator.getEstimatedMyx());
4248         assertNull(calibrator.getEstimatedMyz());
4249         assertNull(calibrator.getEstimatedMzx());
4250         assertNull(calibrator.getEstimatedMzy());
4251         assertNull(calibrator.getEstimatedCovariance());
4252         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4253         assertNull(calibrator.getGroundTruthGravityNorm());
4254         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4255         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4256     }
4257 
4258     @Test
4259     public void testConstructor40() throws WrongSizeException {
4260         final Matrix ba = generateBa();
4261         final double biasX = ba.getElementAtIndex(0);
4262         final double biasY = ba.getElementAtIndex(1);
4263         final double biasZ = ba.getElementAtIndex(2);
4264 
4265         final Matrix ma = generateMaCommonAxis();
4266         final double sx = ma.getElementAt(0, 0);
4267         final double sy = ma.getElementAt(1, 1);
4268         final double sz = ma.getElementAt(2, 2);
4269         final double mxy = ma.getElementAt(0, 1);
4270         final double mxz = ma.getElementAt(0, 2);
4271         final double myx = ma.getElementAt(1, 0);
4272         final double myz = ma.getElementAt(1, 2);
4273         final double mzx = ma.getElementAt(2, 0);
4274         final double mzy = ma.getElementAt(2, 1);
4275 
4276         final KnownPositionAccelerometerCalibrator calibrator =
4277                 new KnownPositionAccelerometerCalibrator(biasX, biasY, biasZ,
4278                         sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
4279 
4280         // check default values
4281         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4282         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4283         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4284         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4285         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4286         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4287         final Acceleration bx2 = new Acceleration(0.0,
4288                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4289         calibrator.getInitialBiasXAsAcceleration(bx2);
4290         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4291         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4292         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4293         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4294         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4295         final Acceleration by2 = new Acceleration(0.0,
4296                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4297         calibrator.getInitialBiasYAsAcceleration(by2);
4298         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4299         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4300         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4301         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4302         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4303         final Acceleration bz2 = new Acceleration(0.0,
4304                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4305         calibrator.getInitialBiasZAsAcceleration(bz2);
4306         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4307         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4308         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4309         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4310         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4311         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4312         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4313         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4314         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4315         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4316         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4317         final double[] bias1 = calibrator.getInitialBias();
4318         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4319         final double[] bias2 = new double[3];
4320         calibrator.getInitialBias(bias2);
4321         assertArrayEquals(bias1, bias2, 0.0);
4322         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4323         assertEquals(b1, ba);
4324         final Matrix b2 = new Matrix(3, 1);
4325         calibrator.getInitialBiasAsMatrix(b2);
4326         assertEquals(b1, b2);
4327         final Matrix ma1 = new Matrix(3, 3);
4328         ma1.setSubmatrix(0, 0,
4329                 2, 2,
4330                 new double[]{sx, myx, mzx,
4331                         mxy, sy, mzy,
4332                         mxz, myz, sz});
4333         assertEquals(calibrator.getInitialMa(), ma1);
4334         final Matrix ma2 = new Matrix(3, 3);
4335         calibrator.getInitialMa(ma2);
4336         assertEquals(ma1, ma2);
4337         assertNull(calibrator.getEcefPosition());
4338         assertNull(calibrator.getNedPosition());
4339         assertFalse(calibrator.getNedPosition(null));
4340         assertNull(calibrator.getMeasurements());
4341         assertFalse(calibrator.isCommonAxisUsed());
4342         assertNull(calibrator.getListener());
4343         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
4344         assertFalse(calibrator.isReady());
4345         assertFalse(calibrator.isRunning());
4346         assertNull(calibrator.getEstimatedBiases());
4347         assertFalse(calibrator.getEstimatedBiases(null));
4348         assertNull(calibrator.getEstimatedBiasesAsMatrix());
4349         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4350         assertNull(calibrator.getEstimatedBiasFx());
4351         assertNull(calibrator.getEstimatedBiasFy());
4352         assertNull(calibrator.getEstimatedBiasFz());
4353         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4354         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4355         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4356         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4357         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4358         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4359         assertNull(calibrator.getEstimatedMa());
4360         assertNull(calibrator.getEstimatedSx());
4361         assertNull(calibrator.getEstimatedSy());
4362         assertNull(calibrator.getEstimatedSz());
4363         assertNull(calibrator.getEstimatedMxy());
4364         assertNull(calibrator.getEstimatedMxz());
4365         assertNull(calibrator.getEstimatedMyx());
4366         assertNull(calibrator.getEstimatedMyz());
4367         assertNull(calibrator.getEstimatedMzx());
4368         assertNull(calibrator.getEstimatedMzy());
4369         assertNull(calibrator.getEstimatedCovariance());
4370         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4371         assertNull(calibrator.getGroundTruthGravityNorm());
4372         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4373         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4374     }
4375 
4376     @Test
4377     public void testConstructor41() throws WrongSizeException {
4378         final Collection<StandardDeviationBodyKinematics> measurements =
4379                 Collections.emptyList();
4380 
4381         final Matrix ba = generateBa();
4382         final double biasX = ba.getElementAtIndex(0);
4383         final double biasY = ba.getElementAtIndex(1);
4384         final double biasZ = ba.getElementAtIndex(2);
4385 
4386         final Matrix ma = generateMaCommonAxis();
4387         final double sx = ma.getElementAt(0, 0);
4388         final double sy = ma.getElementAt(1, 1);
4389         final double sz = ma.getElementAt(2, 2);
4390         final double mxy = ma.getElementAt(0, 1);
4391         final double mxz = ma.getElementAt(0, 2);
4392         final double myx = ma.getElementAt(1, 0);
4393         final double myz = ma.getElementAt(1, 2);
4394         final double mzx = ma.getElementAt(2, 0);
4395         final double mzy = ma.getElementAt(2, 1);
4396 
4397         final KnownPositionAccelerometerCalibrator calibrator =
4398                 new KnownPositionAccelerometerCalibrator(measurements,
4399                         biasX, biasY, biasZ, sx, sy, sz,
4400                         mxy, mxz, myx, myz, mzx, mzy);
4401 
4402         // check default values
4403         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4404         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4405         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4406         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4407         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4408         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4409         final Acceleration bx2 = new Acceleration(0.0,
4410                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4411         calibrator.getInitialBiasXAsAcceleration(bx2);
4412         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4413         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4414         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4415         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4416         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4417         final Acceleration by2 = new Acceleration(0.0,
4418                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4419         calibrator.getInitialBiasYAsAcceleration(by2);
4420         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4421         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4422         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4423         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4424         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4425         final Acceleration bz2 = new Acceleration(0.0,
4426                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4427         calibrator.getInitialBiasZAsAcceleration(bz2);
4428         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4429         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4430         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4431         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4432         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4433         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4434         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4435         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4436         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4437         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4438         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4439         final double[] bias1 = calibrator.getInitialBias();
4440         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4441         final double[] bias2 = new double[3];
4442         calibrator.getInitialBias(bias2);
4443         assertArrayEquals(bias1, bias2, 0.0);
4444         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4445         assertEquals(b1, ba);
4446         final Matrix b2 = new Matrix(3, 1);
4447         calibrator.getInitialBiasAsMatrix(b2);
4448         assertEquals(b1, b2);
4449         final Matrix ma1 = new Matrix(3, 3);
4450         ma1.setSubmatrix(0, 0,
4451                 2, 2,
4452                 new double[]{sx, myx, mzx,
4453                         mxy, sy, mzy,
4454                         mxz, myz, sz});
4455         assertEquals(calibrator.getInitialMa(), ma1);
4456         final Matrix ma2 = new Matrix(3, 3);
4457         calibrator.getInitialMa(ma2);
4458         assertEquals(ma1, ma2);
4459         assertNull(calibrator.getEcefPosition());
4460         assertNull(calibrator.getNedPosition());
4461         assertFalse(calibrator.getNedPosition(null));
4462         assertSame(calibrator.getMeasurements(), measurements);
4463         assertFalse(calibrator.isCommonAxisUsed());
4464         assertNull(calibrator.getListener());
4465         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
4466         assertFalse(calibrator.isReady());
4467         assertFalse(calibrator.isRunning());
4468         assertNull(calibrator.getEstimatedBiases());
4469         assertFalse(calibrator.getEstimatedBiases(null));
4470         assertNull(calibrator.getEstimatedBiasesAsMatrix());
4471         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4472         assertNull(calibrator.getEstimatedBiasFx());
4473         assertNull(calibrator.getEstimatedBiasFy());
4474         assertNull(calibrator.getEstimatedBiasFz());
4475         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4476         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4477         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4478         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4479         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4480         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4481         assertNull(calibrator.getEstimatedMa());
4482         assertNull(calibrator.getEstimatedSx());
4483         assertNull(calibrator.getEstimatedSy());
4484         assertNull(calibrator.getEstimatedSz());
4485         assertNull(calibrator.getEstimatedMxy());
4486         assertNull(calibrator.getEstimatedMxz());
4487         assertNull(calibrator.getEstimatedMyx());
4488         assertNull(calibrator.getEstimatedMyz());
4489         assertNull(calibrator.getEstimatedMzx());
4490         assertNull(calibrator.getEstimatedMzy());
4491         assertNull(calibrator.getEstimatedCovariance());
4492         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4493         assertNull(calibrator.getGroundTruthGravityNorm());
4494         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4495         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4496     }
4497 
4498     @Test
4499     public void testConstructor42() throws WrongSizeException {
4500         final Collection<StandardDeviationBodyKinematics> measurements =
4501                 Collections.emptyList();
4502 
4503         final Matrix ba = generateBa();
4504         final double biasX = ba.getElementAtIndex(0);
4505         final double biasY = ba.getElementAtIndex(1);
4506         final double biasZ = ba.getElementAtIndex(2);
4507 
4508         final Matrix ma = generateMaCommonAxis();
4509         final double sx = ma.getElementAt(0, 0);
4510         final double sy = ma.getElementAt(1, 1);
4511         final double sz = ma.getElementAt(2, 2);
4512         final double mxy = ma.getElementAt(0, 1);
4513         final double mxz = ma.getElementAt(0, 2);
4514         final double myx = ma.getElementAt(1, 0);
4515         final double myz = ma.getElementAt(1, 2);
4516         final double mzx = ma.getElementAt(2, 0);
4517         final double mzy = ma.getElementAt(2, 1);
4518 
4519         final KnownPositionAccelerometerCalibrator calibrator =
4520                 new KnownPositionAccelerometerCalibrator(measurements,
4521                         biasX, biasY, biasZ, sx, sy, sz,
4522                         mxy, mxz, myx, myz, mzx, mzy, this);
4523 
4524         // check default values
4525         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4526         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4527         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4528         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4529         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4530         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4531         final Acceleration bx2 = new Acceleration(0.0,
4532                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4533         calibrator.getInitialBiasXAsAcceleration(bx2);
4534         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4535         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4536         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4537         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4538         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4539         final Acceleration by2 = new Acceleration(0.0,
4540                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4541         calibrator.getInitialBiasYAsAcceleration(by2);
4542         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4543         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4544         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4545         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4546         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4547         final Acceleration bz2 = new Acceleration(0.0,
4548                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4549         calibrator.getInitialBiasZAsAcceleration(bz2);
4550         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4551         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4552         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4553         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4554         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4555         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4556         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4557         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4558         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4559         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4560         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4561         final double[] bias1 = calibrator.getInitialBias();
4562         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4563         final double[] bias2 = new double[3];
4564         calibrator.getInitialBias(bias2);
4565         assertArrayEquals(bias1, bias2, 0.0);
4566         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4567         assertEquals(b1, ba);
4568         final Matrix b2 = new Matrix(3, 1);
4569         calibrator.getInitialBiasAsMatrix(b2);
4570         assertEquals(b1, b2);
4571         final Matrix ma1 = new Matrix(3, 3);
4572         ma1.setSubmatrix(0, 0,
4573                 2, 2,
4574                 new double[]{sx, myx, mzx,
4575                         mxy, sy, mzy,
4576                         mxz, myz, sz});
4577         assertEquals(calibrator.getInitialMa(), ma1);
4578         final Matrix ma2 = new Matrix(3, 3);
4579         calibrator.getInitialMa(ma2);
4580         assertEquals(ma1, ma2);
4581         assertNull(calibrator.getEcefPosition());
4582         assertNull(calibrator.getNedPosition());
4583         assertFalse(calibrator.getNedPosition(null));
4584         assertSame(calibrator.getMeasurements(), measurements);
4585         assertFalse(calibrator.isCommonAxisUsed());
4586         assertSame(calibrator.getListener(), this);
4587         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
4588         assertFalse(calibrator.isReady());
4589         assertFalse(calibrator.isRunning());
4590         assertNull(calibrator.getEstimatedBiases());
4591         assertFalse(calibrator.getEstimatedBiases(null));
4592         assertNull(calibrator.getEstimatedBiasesAsMatrix());
4593         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4594         assertNull(calibrator.getEstimatedBiasFx());
4595         assertNull(calibrator.getEstimatedBiasFy());
4596         assertNull(calibrator.getEstimatedBiasFz());
4597         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4598         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4599         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4600         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4601         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4602         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4603         assertNull(calibrator.getEstimatedMa());
4604         assertNull(calibrator.getEstimatedSx());
4605         assertNull(calibrator.getEstimatedSy());
4606         assertNull(calibrator.getEstimatedSz());
4607         assertNull(calibrator.getEstimatedMxy());
4608         assertNull(calibrator.getEstimatedMxz());
4609         assertNull(calibrator.getEstimatedMyx());
4610         assertNull(calibrator.getEstimatedMyz());
4611         assertNull(calibrator.getEstimatedMzx());
4612         assertNull(calibrator.getEstimatedMzy());
4613         assertNull(calibrator.getEstimatedCovariance());
4614         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4615         assertNull(calibrator.getGroundTruthGravityNorm());
4616         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4617         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4618     }
4619 
4620     @Test
4621     public void testConstructor43() throws WrongSizeException {
4622         final Matrix ba = generateBa();
4623         final double biasX = ba.getElementAtIndex(0);
4624         final double biasY = ba.getElementAtIndex(1);
4625         final double biasZ = ba.getElementAtIndex(2);
4626 
4627         final Matrix ma = generateMaCommonAxis();
4628         final double sx = ma.getElementAt(0, 0);
4629         final double sy = ma.getElementAt(1, 1);
4630         final double sz = ma.getElementAt(2, 2);
4631         final double mxy = ma.getElementAt(0, 1);
4632         final double mxz = ma.getElementAt(0, 2);
4633         final double myx = ma.getElementAt(1, 0);
4634         final double myz = ma.getElementAt(1, 2);
4635         final double mzx = ma.getElementAt(2, 0);
4636         final double mzy = ma.getElementAt(2, 1);
4637 
4638         final KnownPositionAccelerometerCalibrator calibrator =
4639                 new KnownPositionAccelerometerCalibrator(true,
4640                         biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx,
4641                         myz, mzx, mzy);
4642 
4643         // check default values
4644         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4645         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4646         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4647         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4648         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4649         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4650         final Acceleration bx2 = new Acceleration(0.0,
4651                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4652         calibrator.getInitialBiasXAsAcceleration(bx2);
4653         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4654         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4655         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4656         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4657         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4658         final Acceleration by2 = new Acceleration(0.0,
4659                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4660         calibrator.getInitialBiasYAsAcceleration(by2);
4661         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4662         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4663         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4664         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4665         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4666         final Acceleration bz2 = new Acceleration(0.0,
4667                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4668         calibrator.getInitialBiasZAsAcceleration(bz2);
4669         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4670         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4671         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4672         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4673         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4674         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4675         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4676         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4677         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4678         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4679         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4680         final double[] bias1 = calibrator.getInitialBias();
4681         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4682         final double[] bias2 = new double[3];
4683         calibrator.getInitialBias(bias2);
4684         assertArrayEquals(bias1, bias2, 0.0);
4685         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4686         assertEquals(b1, ba);
4687         final Matrix b2 = new Matrix(3, 1);
4688         calibrator.getInitialBiasAsMatrix(b2);
4689         assertEquals(b1, b2);
4690         final Matrix ma1 = new Matrix(3, 3);
4691         ma1.setSubmatrix(0, 0,
4692                 2, 2,
4693                 new double[]{sx, myx, mzx,
4694                         mxy, sy, mzy,
4695                         mxz, myz, sz});
4696         assertEquals(calibrator.getInitialMa(), ma1);
4697         final Matrix ma2 = new Matrix(3, 3);
4698         calibrator.getInitialMa(ma2);
4699         assertEquals(ma1, ma2);
4700         assertNull(calibrator.getEcefPosition());
4701         assertNull(calibrator.getNedPosition());
4702         assertFalse(calibrator.getNedPosition(null));
4703         assertNull(calibrator.getMeasurements());
4704         assertTrue(calibrator.isCommonAxisUsed());
4705         assertNull(calibrator.getListener());
4706         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4707         assertFalse(calibrator.isReady());
4708         assertFalse(calibrator.isRunning());
4709         assertNull(calibrator.getEstimatedBiases());
4710         assertFalse(calibrator.getEstimatedBiases(null));
4711         assertNull(calibrator.getEstimatedBiasesAsMatrix());
4712         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4713         assertNull(calibrator.getEstimatedBiasFx());
4714         assertNull(calibrator.getEstimatedBiasFy());
4715         assertNull(calibrator.getEstimatedBiasFz());
4716         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4717         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4718         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4719         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4720         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4721         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4722         assertNull(calibrator.getEstimatedMa());
4723         assertNull(calibrator.getEstimatedSx());
4724         assertNull(calibrator.getEstimatedSy());
4725         assertNull(calibrator.getEstimatedSz());
4726         assertNull(calibrator.getEstimatedMxy());
4727         assertNull(calibrator.getEstimatedMxz());
4728         assertNull(calibrator.getEstimatedMyx());
4729         assertNull(calibrator.getEstimatedMyz());
4730         assertNull(calibrator.getEstimatedMzx());
4731         assertNull(calibrator.getEstimatedMzy());
4732         assertNull(calibrator.getEstimatedCovariance());
4733         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4734         assertNull(calibrator.getGroundTruthGravityNorm());
4735         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4736         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4737     }
4738 
4739     @Test
4740     public void testConstructor44() throws WrongSizeException {
4741         final Matrix ba = generateBa();
4742         final double biasX = ba.getElementAtIndex(0);
4743         final double biasY = ba.getElementAtIndex(1);
4744         final double biasZ = ba.getElementAtIndex(2);
4745 
4746         final Matrix ma = generateMaCommonAxis();
4747         final double sx = ma.getElementAt(0, 0);
4748         final double sy = ma.getElementAt(1, 1);
4749         final double sz = ma.getElementAt(2, 2);
4750         final double mxy = ma.getElementAt(0, 1);
4751         final double mxz = ma.getElementAt(0, 2);
4752         final double myx = ma.getElementAt(1, 0);
4753         final double myz = ma.getElementAt(1, 2);
4754         final double mzx = ma.getElementAt(2, 0);
4755         final double mzy = ma.getElementAt(2, 1);
4756 
4757         final KnownPositionAccelerometerCalibrator calibrator =
4758                 new KnownPositionAccelerometerCalibrator(true,
4759                         biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx,
4760                         myz, mzx, mzy, this);
4761 
4762         // check default values
4763         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4764         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4765         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4766         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4767         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4768         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4769         final Acceleration bx2 = new Acceleration(0.0,
4770                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4771         calibrator.getInitialBiasXAsAcceleration(bx2);
4772         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4773         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4774         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4775         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4776         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4777         final Acceleration by2 = new Acceleration(0.0,
4778                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4779         calibrator.getInitialBiasYAsAcceleration(by2);
4780         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4781         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4782         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4783         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4784         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4785         final Acceleration bz2 = new Acceleration(0.0,
4786                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4787         calibrator.getInitialBiasZAsAcceleration(bz2);
4788         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4789         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4790         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4791         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4792         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4793         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4794         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4795         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4796         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4797         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4798         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4799         final double[] bias1 = calibrator.getInitialBias();
4800         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4801         final double[] bias2 = new double[3];
4802         calibrator.getInitialBias(bias2);
4803         assertArrayEquals(bias1, bias2, 0.0);
4804         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4805         assertEquals(b1, ba);
4806         final Matrix b2 = new Matrix(3, 1);
4807         calibrator.getInitialBiasAsMatrix(b2);
4808         assertEquals(b1, b2);
4809         final Matrix ma1 = new Matrix(3, 3);
4810         ma1.setSubmatrix(0, 0,
4811                 2, 2,
4812                 new double[]{sx, myx, mzx,
4813                         mxy, sy, mzy,
4814                         mxz, myz, sz});
4815         assertEquals(calibrator.getInitialMa(), ma1);
4816         final Matrix ma2 = new Matrix(3, 3);
4817         calibrator.getInitialMa(ma2);
4818         assertEquals(ma1, ma2);
4819         assertNull(calibrator.getEcefPosition());
4820         assertNull(calibrator.getNedPosition());
4821         assertFalse(calibrator.getNedPosition(null));
4822         assertNull(calibrator.getMeasurements());
4823         assertTrue(calibrator.isCommonAxisUsed());
4824         assertSame(calibrator.getListener(), this);
4825         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4826         assertFalse(calibrator.isReady());
4827         assertFalse(calibrator.isRunning());
4828         assertNull(calibrator.getEstimatedBiases());
4829         assertFalse(calibrator.getEstimatedBiases(null));
4830         assertNull(calibrator.getEstimatedBiasesAsMatrix());
4831         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4832         assertNull(calibrator.getEstimatedBiasFx());
4833         assertNull(calibrator.getEstimatedBiasFy());
4834         assertNull(calibrator.getEstimatedBiasFz());
4835         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4836         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4837         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4838         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4839         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4840         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4841         assertNull(calibrator.getEstimatedMa());
4842         assertNull(calibrator.getEstimatedSx());
4843         assertNull(calibrator.getEstimatedSy());
4844         assertNull(calibrator.getEstimatedSz());
4845         assertNull(calibrator.getEstimatedMxy());
4846         assertNull(calibrator.getEstimatedMxz());
4847         assertNull(calibrator.getEstimatedMyx());
4848         assertNull(calibrator.getEstimatedMyz());
4849         assertNull(calibrator.getEstimatedMzx());
4850         assertNull(calibrator.getEstimatedMzy());
4851         assertNull(calibrator.getEstimatedCovariance());
4852         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4853         assertNull(calibrator.getGroundTruthGravityNorm());
4854         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4855         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4856     }
4857 
4858     @Test
4859     public void testConstructor45() throws WrongSizeException {
4860         final Collection<StandardDeviationBodyKinematics> measurements =
4861                 Collections.emptyList();
4862 
4863         final Matrix ba = generateBa();
4864         final double biasX = ba.getElementAtIndex(0);
4865         final double biasY = ba.getElementAtIndex(1);
4866         final double biasZ = ba.getElementAtIndex(2);
4867 
4868         final Matrix ma = generateMaCommonAxis();
4869         final double sx = ma.getElementAt(0, 0);
4870         final double sy = ma.getElementAt(1, 1);
4871         final double sz = ma.getElementAt(2, 2);
4872         final double mxy = ma.getElementAt(0, 1);
4873         final double mxz = ma.getElementAt(0, 2);
4874         final double myx = ma.getElementAt(1, 0);
4875         final double myz = ma.getElementAt(1, 2);
4876         final double mzx = ma.getElementAt(2, 0);
4877         final double mzy = ma.getElementAt(2, 1);
4878 
4879         final KnownPositionAccelerometerCalibrator calibrator =
4880                 new KnownPositionAccelerometerCalibrator(measurements,
4881                         true, biasX, biasY, biasZ,
4882                         sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
4883 
4884         // check default values
4885         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4886         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4887         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4888         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4889         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4890         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4891         final Acceleration bx2 = new Acceleration(0.0,
4892                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4893         calibrator.getInitialBiasXAsAcceleration(bx2);
4894         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4895         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4896         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4897         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4898         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4899         final Acceleration by2 = new Acceleration(0.0,
4900                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4901         calibrator.getInitialBiasYAsAcceleration(by2);
4902         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4903         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4904         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4905         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4906         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4907         final Acceleration bz2 = new Acceleration(0.0,
4908                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4909         calibrator.getInitialBiasZAsAcceleration(bz2);
4910         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4911         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4912         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4913         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4914         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4915         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4916         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4917         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4918         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4919         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4920         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4921         final double[] bias1 = calibrator.getInitialBias();
4922         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4923         final double[] bias2 = new double[3];
4924         calibrator.getInitialBias(bias2);
4925         assertArrayEquals(bias1, bias2, 0.0);
4926         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4927         assertEquals(b1, ba);
4928         final Matrix b2 = new Matrix(3, 1);
4929         calibrator.getInitialBiasAsMatrix(b2);
4930         assertEquals(b1, b2);
4931         final Matrix ma1 = new Matrix(3, 3);
4932         ma1.setSubmatrix(0, 0,
4933                 2, 2,
4934                 new double[]{sx, myx, mzx,
4935                         mxy, sy, mzy,
4936                         mxz, myz, sz});
4937         assertEquals(calibrator.getInitialMa(), ma1);
4938         final Matrix ma2 = new Matrix(3, 3);
4939         calibrator.getInitialMa(ma2);
4940         assertEquals(ma1, ma2);
4941         assertNull(calibrator.getEcefPosition());
4942         assertNull(calibrator.getNedPosition());
4943         assertFalse(calibrator.getNedPosition(null));
4944         assertSame(calibrator.getMeasurements(), measurements);
4945         assertTrue(calibrator.isCommonAxisUsed());
4946         assertNull(calibrator.getListener());
4947         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4948         assertFalse(calibrator.isReady());
4949         assertFalse(calibrator.isRunning());
4950         assertNull(calibrator.getEstimatedBiases());
4951         assertFalse(calibrator.getEstimatedBiases(null));
4952         assertNull(calibrator.getEstimatedBiasesAsMatrix());
4953         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4954         assertNull(calibrator.getEstimatedBiasFx());
4955         assertNull(calibrator.getEstimatedBiasFy());
4956         assertNull(calibrator.getEstimatedBiasFz());
4957         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4958         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4959         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4960         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4961         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4962         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4963         assertNull(calibrator.getEstimatedMa());
4964         assertNull(calibrator.getEstimatedSx());
4965         assertNull(calibrator.getEstimatedSy());
4966         assertNull(calibrator.getEstimatedSz());
4967         assertNull(calibrator.getEstimatedMxy());
4968         assertNull(calibrator.getEstimatedMxz());
4969         assertNull(calibrator.getEstimatedMyx());
4970         assertNull(calibrator.getEstimatedMyz());
4971         assertNull(calibrator.getEstimatedMzx());
4972         assertNull(calibrator.getEstimatedMzy());
4973         assertNull(calibrator.getEstimatedCovariance());
4974         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4975         assertNull(calibrator.getGroundTruthGravityNorm());
4976         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4977         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4978     }
4979 
4980     @Test
4981     public void testConstructor46() throws WrongSizeException {
4982         final Collection<StandardDeviationBodyKinematics> measurements =
4983                 Collections.emptyList();
4984 
4985         final Matrix ba = generateBa();
4986         final double biasX = ba.getElementAtIndex(0);
4987         final double biasY = ba.getElementAtIndex(1);
4988         final double biasZ = ba.getElementAtIndex(2);
4989 
4990         final Matrix ma = generateMaCommonAxis();
4991         final double sx = ma.getElementAt(0, 0);
4992         final double sy = ma.getElementAt(1, 1);
4993         final double sz = ma.getElementAt(2, 2);
4994         final double mxy = ma.getElementAt(0, 1);
4995         final double mxz = ma.getElementAt(0, 2);
4996         final double myx = ma.getElementAt(1, 0);
4997         final double myz = ma.getElementAt(1, 2);
4998         final double mzx = ma.getElementAt(2, 0);
4999         final double mzy = ma.getElementAt(2, 1);
5000 
5001         final KnownPositionAccelerometerCalibrator calibrator =
5002                 new KnownPositionAccelerometerCalibrator(measurements,
5003                         true, biasX, biasY, biasZ,
5004                         sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
5005 
5006         // check default values
5007         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5008         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5009         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5010         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5011         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5012         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5013         final Acceleration bx2 = new Acceleration(0.0,
5014                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5015         calibrator.getInitialBiasXAsAcceleration(bx2);
5016         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5017         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5018         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5019         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5020         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5021         final Acceleration by2 = new Acceleration(0.0,
5022                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5023         calibrator.getInitialBiasYAsAcceleration(by2);
5024         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5025         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5026         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5027         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5028         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5029         final Acceleration bz2 = new Acceleration(0.0,
5030                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5031         calibrator.getInitialBiasZAsAcceleration(bz2);
5032         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5033         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5034         assertEquals(calibrator.getInitialSx(), sx, 0.0);
5035         assertEquals(calibrator.getInitialSy(), sy, 0.0);
5036         assertEquals(calibrator.getInitialSz(), sz, 0.0);
5037         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5038         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5039         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5040         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5041         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5042         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5043         final double[] bias1 = calibrator.getInitialBias();
5044         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5045         final double[] bias2 = new double[3];
5046         calibrator.getInitialBias(bias2);
5047         assertArrayEquals(bias1, bias2, 0.0);
5048         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5049         assertEquals(b1, ba);
5050         final Matrix b2 = new Matrix(3, 1);
5051         calibrator.getInitialBiasAsMatrix(b2);
5052         assertEquals(b1, b2);
5053         final Matrix ma1 = new Matrix(3, 3);
5054         ma1.setSubmatrix(0, 0,
5055                 2, 2,
5056                 new double[]{sx, myx, mzx,
5057                         mxy, sy, mzy,
5058                         mxz, myz, sz});
5059         assertEquals(calibrator.getInitialMa(), ma1);
5060         final Matrix ma2 = new Matrix(3, 3);
5061         calibrator.getInitialMa(ma2);
5062         assertEquals(ma1, ma2);
5063         assertNull(calibrator.getEcefPosition());
5064         assertNull(calibrator.getNedPosition());
5065         assertFalse(calibrator.getNedPosition(null));
5066         assertSame(calibrator.getMeasurements(), measurements);
5067         assertTrue(calibrator.isCommonAxisUsed());
5068         assertSame(calibrator.getListener(), this);
5069         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
5070         assertFalse(calibrator.isReady());
5071         assertFalse(calibrator.isRunning());
5072         assertNull(calibrator.getEstimatedBiases());
5073         assertFalse(calibrator.getEstimatedBiases(null));
5074         assertNull(calibrator.getEstimatedBiasesAsMatrix());
5075         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5076         assertNull(calibrator.getEstimatedBiasFx());
5077         assertNull(calibrator.getEstimatedBiasFy());
5078         assertNull(calibrator.getEstimatedBiasFz());
5079         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5080         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5081         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5082         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5083         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5084         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5085         assertNull(calibrator.getEstimatedMa());
5086         assertNull(calibrator.getEstimatedSx());
5087         assertNull(calibrator.getEstimatedSy());
5088         assertNull(calibrator.getEstimatedSz());
5089         assertNull(calibrator.getEstimatedMxy());
5090         assertNull(calibrator.getEstimatedMxz());
5091         assertNull(calibrator.getEstimatedMyx());
5092         assertNull(calibrator.getEstimatedMyz());
5093         assertNull(calibrator.getEstimatedMzx());
5094         assertNull(calibrator.getEstimatedMzy());
5095         assertNull(calibrator.getEstimatedCovariance());
5096         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5097         assertNull(calibrator.getGroundTruthGravityNorm());
5098         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5099         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5100     }
5101 
5102     @Test
5103     public void testConstructor47() throws WrongSizeException {
5104         final Matrix ba = generateBa();
5105         final double biasX = ba.getElementAtIndex(0);
5106         final double biasY = ba.getElementAtIndex(1);
5107         final double biasZ = ba.getElementAtIndex(2);
5108 
5109         final Matrix ma = generateMaCommonAxis();
5110         final double sx = ma.getElementAt(0, 0);
5111         final double sy = ma.getElementAt(1, 1);
5112         final double sz = ma.getElementAt(2, 2);
5113         final double mxy = ma.getElementAt(0, 1);
5114         final double mxz = ma.getElementAt(0, 2);
5115         final double myx = ma.getElementAt(1, 0);
5116         final double myz = ma.getElementAt(1, 2);
5117         final double mzx = ma.getElementAt(2, 0);
5118         final double mzy = ma.getElementAt(2, 1);
5119 
5120         final Acceleration bx = new Acceleration(biasX,
5121                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5122         final Acceleration by = new Acceleration(biasY,
5123                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5124         final Acceleration bz = new Acceleration(biasZ,
5125                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5126 
5127         final KnownPositionAccelerometerCalibrator calibrator =
5128                 new KnownPositionAccelerometerCalibrator(bx, by, bz,
5129                         sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
5130 
5131         // check default values
5132         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5133         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5134         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5135         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5136         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5137         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5138         final Acceleration bx2 = new Acceleration(0.0,
5139                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5140         calibrator.getInitialBiasXAsAcceleration(bx2);
5141         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5142         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5143         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5144         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5145         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5146         final Acceleration by2 = new Acceleration(0.0,
5147                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5148         calibrator.getInitialBiasYAsAcceleration(by2);
5149         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5150         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5151         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5152         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5153         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5154         final Acceleration bz2 = new Acceleration(0.0,
5155                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5156         calibrator.getInitialBiasZAsAcceleration(bz2);
5157         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5158         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5159         assertEquals(calibrator.getInitialSx(), sx, 0.0);
5160         assertEquals(calibrator.getInitialSy(), sy, 0.0);
5161         assertEquals(calibrator.getInitialSz(), sz, 0.0);
5162         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5163         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5164         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5165         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5166         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5167         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5168         final double[] bias1 = calibrator.getInitialBias();
5169         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5170         final double[] bias2 = new double[3];
5171         calibrator.getInitialBias(bias2);
5172         assertArrayEquals(bias1, bias2, 0.0);
5173         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5174         assertEquals(b1, ba);
5175         final Matrix b2 = new Matrix(3, 1);
5176         calibrator.getInitialBiasAsMatrix(b2);
5177         assertEquals(b1, b2);
5178         final Matrix ma1 = new Matrix(3, 3);
5179         ma1.setSubmatrix(0, 0,
5180                 2, 2,
5181                 new double[]{sx, myx, mzx,
5182                         mxy, sy, mzy,
5183                         mxz, myz, sz});
5184         assertEquals(calibrator.getInitialMa(), ma1);
5185         final Matrix ma2 = new Matrix(3, 3);
5186         calibrator.getInitialMa(ma2);
5187         assertEquals(ma1, ma2);
5188         assertNull(calibrator.getEcefPosition());
5189         assertNull(calibrator.getNedPosition());
5190         assertFalse(calibrator.getNedPosition(null));
5191         assertNull(calibrator.getMeasurements());
5192         assertFalse(calibrator.isCommonAxisUsed());
5193         assertNull(calibrator.getListener());
5194         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
5195         assertFalse(calibrator.isReady());
5196         assertFalse(calibrator.isRunning());
5197         assertNull(calibrator.getEstimatedBiases());
5198         assertFalse(calibrator.getEstimatedBiases(null));
5199         assertNull(calibrator.getEstimatedBiasesAsMatrix());
5200         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5201         assertNull(calibrator.getEstimatedBiasFx());
5202         assertNull(calibrator.getEstimatedBiasFy());
5203         assertNull(calibrator.getEstimatedBiasFz());
5204         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5205         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5206         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5207         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5208         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5209         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5210         assertNull(calibrator.getEstimatedMa());
5211         assertNull(calibrator.getEstimatedSx());
5212         assertNull(calibrator.getEstimatedSy());
5213         assertNull(calibrator.getEstimatedSz());
5214         assertNull(calibrator.getEstimatedMxy());
5215         assertNull(calibrator.getEstimatedMxz());
5216         assertNull(calibrator.getEstimatedMyx());
5217         assertNull(calibrator.getEstimatedMyz());
5218         assertNull(calibrator.getEstimatedMzx());
5219         assertNull(calibrator.getEstimatedMzy());
5220         assertNull(calibrator.getEstimatedCovariance());
5221         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5222         assertNull(calibrator.getGroundTruthGravityNorm());
5223         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5224         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5225     }
5226 
5227     @Test
5228     public void testConstructor48() throws WrongSizeException {
5229         final Matrix ba = generateBa();
5230         final double biasX = ba.getElementAtIndex(0);
5231         final double biasY = ba.getElementAtIndex(1);
5232         final double biasZ = ba.getElementAtIndex(2);
5233 
5234         final Matrix ma = generateMaCommonAxis();
5235         final double sx = ma.getElementAt(0, 0);
5236         final double sy = ma.getElementAt(1, 1);
5237         final double sz = ma.getElementAt(2, 2);
5238         final double mxy = ma.getElementAt(0, 1);
5239         final double mxz = ma.getElementAt(0, 2);
5240         final double myx = ma.getElementAt(1, 0);
5241         final double myz = ma.getElementAt(1, 2);
5242         final double mzx = ma.getElementAt(2, 0);
5243         final double mzy = ma.getElementAt(2, 1);
5244 
5245         final Acceleration bx = new Acceleration(biasX,
5246                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5247         final Acceleration by = new Acceleration(biasY,
5248                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5249         final Acceleration bz = new Acceleration(biasZ,
5250                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5251 
5252         final KnownPositionAccelerometerCalibrator calibrator =
5253                 new KnownPositionAccelerometerCalibrator(bx, by, bz,
5254                         sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
5255 
5256         // check default values
5257         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5258         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5259         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5260         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5261         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5262         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5263         final Acceleration bx2 = new Acceleration(0.0,
5264                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5265         calibrator.getInitialBiasXAsAcceleration(bx2);
5266         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5267         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5268         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5269         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5270         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5271         final Acceleration by2 = new Acceleration(0.0,
5272                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5273         calibrator.getInitialBiasYAsAcceleration(by2);
5274         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5275         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5276         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5277         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5278         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5279         final Acceleration bz2 = new Acceleration(0.0,
5280                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5281         calibrator.getInitialBiasZAsAcceleration(bz2);
5282         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5283         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5284         assertEquals(calibrator.getInitialSx(), sx, 0.0);
5285         assertEquals(calibrator.getInitialSy(), sy, 0.0);
5286         assertEquals(calibrator.getInitialSz(), sz, 0.0);
5287         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5288         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5289         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5290         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5291         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5292         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5293         final double[] bias1 = calibrator.getInitialBias();
5294         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5295         final double[] bias2 = new double[3];
5296         calibrator.getInitialBias(bias2);
5297         assertArrayEquals(bias1, bias2, 0.0);
5298         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5299         assertEquals(b1, ba);
5300         final Matrix b2 = new Matrix(3, 1);
5301         calibrator.getInitialBiasAsMatrix(b2);
5302         assertEquals(b1, b2);
5303         final Matrix ma1 = new Matrix(3, 3);
5304         ma1.setSubmatrix(0, 0,
5305                 2, 2,
5306                 new double[]{sx, myx, mzx,
5307                         mxy, sy, mzy,
5308                         mxz, myz, sz});
5309         assertEquals(calibrator.getInitialMa(), ma1);
5310         final Matrix ma2 = new Matrix(3, 3);
5311         calibrator.getInitialMa(ma2);
5312         assertEquals(ma1, ma2);
5313         assertNull(calibrator.getEcefPosition());
5314         assertNull(calibrator.getNedPosition());
5315         assertFalse(calibrator.getNedPosition(null));
5316         assertNull(calibrator.getMeasurements());
5317         assertFalse(calibrator.isCommonAxisUsed());
5318         assertSame(calibrator.getListener(), this);
5319         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
5320         assertFalse(calibrator.isReady());
5321         assertFalse(calibrator.isRunning());
5322         assertNull(calibrator.getEstimatedBiases());
5323         assertFalse(calibrator.getEstimatedBiases(null));
5324         assertNull(calibrator.getEstimatedBiasesAsMatrix());
5325         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5326         assertNull(calibrator.getEstimatedBiasFx());
5327         assertNull(calibrator.getEstimatedBiasFy());
5328         assertNull(calibrator.getEstimatedBiasFz());
5329         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5330         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5331         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5332         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5333         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5334         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5335         assertNull(calibrator.getEstimatedMa());
5336         assertNull(calibrator.getEstimatedSx());
5337         assertNull(calibrator.getEstimatedSy());
5338         assertNull(calibrator.getEstimatedSz());
5339         assertNull(calibrator.getEstimatedMxy());
5340         assertNull(calibrator.getEstimatedMxz());
5341         assertNull(calibrator.getEstimatedMyx());
5342         assertNull(calibrator.getEstimatedMyz());
5343         assertNull(calibrator.getEstimatedMzx());
5344         assertNull(calibrator.getEstimatedMzy());
5345         assertNull(calibrator.getEstimatedCovariance());
5346         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5347         assertNull(calibrator.getGroundTruthGravityNorm());
5348         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5349         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5350     }
5351 
5352     @Test
5353     public void testConstructor49() throws WrongSizeException {
5354 
5355         final Collection<StandardDeviationBodyKinematics> measurements =
5356                 Collections.emptyList();
5357 
5358         final Matrix ba = generateBa();
5359         final double biasX = ba.getElementAtIndex(0);
5360         final double biasY = ba.getElementAtIndex(1);
5361         final double biasZ = ba.getElementAtIndex(2);
5362 
5363         final Matrix ma = generateMaCommonAxis();
5364         final double sx = ma.getElementAt(0, 0);
5365         final double sy = ma.getElementAt(1, 1);
5366         final double sz = ma.getElementAt(2, 2);
5367         final double mxy = ma.getElementAt(0, 1);
5368         final double mxz = ma.getElementAt(0, 2);
5369         final double myx = ma.getElementAt(1, 0);
5370         final double myz = ma.getElementAt(1, 2);
5371         final double mzx = ma.getElementAt(2, 0);
5372         final double mzy = ma.getElementAt(2, 1);
5373 
5374         final Acceleration bx = new Acceleration(biasX,
5375                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5376         final Acceleration by = new Acceleration(biasY,
5377                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5378         final Acceleration bz = new Acceleration(biasZ,
5379                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5380 
5381         final KnownPositionAccelerometerCalibrator calibrator =
5382                 new KnownPositionAccelerometerCalibrator(measurements, bx, by, bz,
5383                         sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
5384 
5385         // check default values
5386         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5387         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5388         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5389         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5390         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5391         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5392         final Acceleration bx2 = new Acceleration(0.0,
5393                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5394         calibrator.getInitialBiasXAsAcceleration(bx2);
5395         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5396         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5397         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5398         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5399         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5400         final Acceleration by2 = new Acceleration(0.0,
5401                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5402         calibrator.getInitialBiasYAsAcceleration(by2);
5403         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5404         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5405         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5406         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5407         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5408         final Acceleration bz2 = new Acceleration(0.0,
5409                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5410         calibrator.getInitialBiasZAsAcceleration(bz2);
5411         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5412         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5413         assertEquals(calibrator.getInitialSx(), sx, 0.0);
5414         assertEquals(calibrator.getInitialSy(), sy, 0.0);
5415         assertEquals(calibrator.getInitialSz(), sz, 0.0);
5416         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5417         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5418         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5419         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5420         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5421         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5422         final double[] bias1 = calibrator.getInitialBias();
5423         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5424         final double[] bias2 = new double[3];
5425         calibrator.getInitialBias(bias2);
5426         assertArrayEquals(bias1, bias2, 0.0);
5427         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5428         assertEquals(b1, ba);
5429         final Matrix b2 = new Matrix(3, 1);
5430         calibrator.getInitialBiasAsMatrix(b2);
5431         assertEquals(b1, b2);
5432         final Matrix ma1 = new Matrix(3, 3);
5433         ma1.setSubmatrix(0, 0,
5434                 2, 2,
5435                 new double[]{sx, myx, mzx,
5436                         mxy, sy, mzy,
5437                         mxz, myz, sz});
5438         assertEquals(calibrator.getInitialMa(), ma1);
5439         final Matrix ma2 = new Matrix(3, 3);
5440         calibrator.getInitialMa(ma2);
5441         assertEquals(ma1, ma2);
5442         assertNull(calibrator.getEcefPosition());
5443         assertNull(calibrator.getNedPosition());
5444         assertFalse(calibrator.getNedPosition(null));
5445         assertSame(calibrator.getMeasurements(), measurements);
5446         assertFalse(calibrator.isCommonAxisUsed());
5447         assertNull(calibrator.getListener());
5448         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
5449         assertFalse(calibrator.isReady());
5450         assertFalse(calibrator.isRunning());
5451         assertNull(calibrator.getEstimatedBiases());
5452         assertFalse(calibrator.getEstimatedBiases(null));
5453         assertNull(calibrator.getEstimatedBiasesAsMatrix());
5454         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5455         assertNull(calibrator.getEstimatedBiasFx());
5456         assertNull(calibrator.getEstimatedBiasFy());
5457         assertNull(calibrator.getEstimatedBiasFz());
5458         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5459         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5460         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5461         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5462         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5463         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5464         assertNull(calibrator.getEstimatedMa());
5465         assertNull(calibrator.getEstimatedSx());
5466         assertNull(calibrator.getEstimatedSy());
5467         assertNull(calibrator.getEstimatedSz());
5468         assertNull(calibrator.getEstimatedMxy());
5469         assertNull(calibrator.getEstimatedMxz());
5470         assertNull(calibrator.getEstimatedMyx());
5471         assertNull(calibrator.getEstimatedMyz());
5472         assertNull(calibrator.getEstimatedMzx());
5473         assertNull(calibrator.getEstimatedMzy());
5474         assertNull(calibrator.getEstimatedCovariance());
5475         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5476         assertNull(calibrator.getGroundTruthGravityNorm());
5477         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5478         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5479     }
5480 
5481     @Test
5482     public void testConstructor50() throws WrongSizeException {
5483 
5484         final Collection<StandardDeviationBodyKinematics> measurements =
5485                 Collections.emptyList();
5486 
5487         final Matrix ba = generateBa();
5488         final double biasX = ba.getElementAtIndex(0);
5489         final double biasY = ba.getElementAtIndex(1);
5490         final double biasZ = ba.getElementAtIndex(2);
5491 
5492         final Matrix ma = generateMaCommonAxis();
5493         final double sx = ma.getElementAt(0, 0);
5494         final double sy = ma.getElementAt(1, 1);
5495         final double sz = ma.getElementAt(2, 2);
5496         final double mxy = ma.getElementAt(0, 1);
5497         final double mxz = ma.getElementAt(0, 2);
5498         final double myx = ma.getElementAt(1, 0);
5499         final double myz = ma.getElementAt(1, 2);
5500         final double mzx = ma.getElementAt(2, 0);
5501         final double mzy = ma.getElementAt(2, 1);
5502 
5503         final Acceleration bx = new Acceleration(biasX,
5504                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5505         final Acceleration by = new Acceleration(biasY,
5506                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5507         final Acceleration bz = new Acceleration(biasZ,
5508                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5509 
5510         final KnownPositionAccelerometerCalibrator calibrator =
5511                 new KnownPositionAccelerometerCalibrator(measurements, bx, by, bz,
5512                         sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
5513 
5514         // check default values
5515         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5516         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5517         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5518         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5519         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5520         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5521         final Acceleration bx2 = new Acceleration(0.0,
5522                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5523         calibrator.getInitialBiasXAsAcceleration(bx2);
5524         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5525         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5526         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5527         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5528         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5529         final Acceleration by2 = new Acceleration(0.0,
5530                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5531         calibrator.getInitialBiasYAsAcceleration(by2);
5532         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5533         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5534         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5535         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5536         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5537         final Acceleration bz2 = new Acceleration(0.0,
5538                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5539         calibrator.getInitialBiasZAsAcceleration(bz2);
5540         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5541         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5542         assertEquals(calibrator.getInitialSx(), sx, 0.0);
5543         assertEquals(calibrator.getInitialSy(), sy, 0.0);
5544         assertEquals(calibrator.getInitialSz(), sz, 0.0);
5545         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5546         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5547         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5548         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5549         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5550         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5551         final double[] bias1 = calibrator.getInitialBias();
5552         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5553         final double[] bias2 = new double[3];
5554         calibrator.getInitialBias(bias2);
5555         assertArrayEquals(bias1, bias2, 0.0);
5556         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5557         assertEquals(b1, ba);
5558         final Matrix b2 = new Matrix(3, 1);
5559         calibrator.getInitialBiasAsMatrix(b2);
5560         assertEquals(b1, b2);
5561         final Matrix ma1 = new Matrix(3, 3);
5562         ma1.setSubmatrix(0, 0,
5563                 2, 2,
5564                 new double[]{sx, myx, mzx,
5565                         mxy, sy, mzy,
5566                         mxz, myz, sz});
5567         assertEquals(calibrator.getInitialMa(), ma1);
5568         final Matrix ma2 = new Matrix(3, 3);
5569         calibrator.getInitialMa(ma2);
5570         assertEquals(ma1, ma2);
5571         assertNull(calibrator.getEcefPosition());
5572         assertNull(calibrator.getNedPosition());
5573         assertFalse(calibrator.getNedPosition(null));
5574         assertSame(calibrator.getMeasurements(), measurements);
5575         assertFalse(calibrator.isCommonAxisUsed());
5576         assertSame(calibrator.getListener(), this);
5577         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
5578         assertFalse(calibrator.isReady());
5579         assertFalse(calibrator.isRunning());
5580         assertNull(calibrator.getEstimatedBiases());
5581         assertFalse(calibrator.getEstimatedBiases(null));
5582         assertNull(calibrator.getEstimatedBiasesAsMatrix());
5583         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5584         assertNull(calibrator.getEstimatedBiasFx());
5585         assertNull(calibrator.getEstimatedBiasFy());
5586         assertNull(calibrator.getEstimatedBiasFz());
5587         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5588         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5589         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5590         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5591         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5592         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5593         assertNull(calibrator.getEstimatedMa());
5594         assertNull(calibrator.getEstimatedSx());
5595         assertNull(calibrator.getEstimatedSy());
5596         assertNull(calibrator.getEstimatedSz());
5597         assertNull(calibrator.getEstimatedMxy());
5598         assertNull(calibrator.getEstimatedMxz());
5599         assertNull(calibrator.getEstimatedMyx());
5600         assertNull(calibrator.getEstimatedMyz());
5601         assertNull(calibrator.getEstimatedMzx());
5602         assertNull(calibrator.getEstimatedMzy());
5603         assertNull(calibrator.getEstimatedCovariance());
5604         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5605         assertNull(calibrator.getGroundTruthGravityNorm());
5606         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5607         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5608     }
5609 
5610     @Test
5611     public void testConstructor51() throws WrongSizeException {
5612         final Matrix ba = generateBa();
5613         final double biasX = ba.getElementAtIndex(0);
5614         final double biasY = ba.getElementAtIndex(1);
5615         final double biasZ = ba.getElementAtIndex(2);
5616 
5617         final Matrix ma = generateMaCommonAxis();
5618         final double sx = ma.getElementAt(0, 0);
5619         final double sy = ma.getElementAt(1, 1);
5620         final double sz = ma.getElementAt(2, 2);
5621         final double mxy = ma.getElementAt(0, 1);
5622         final double mxz = ma.getElementAt(0, 2);
5623         final double myx = ma.getElementAt(1, 0);
5624         final double myz = ma.getElementAt(1, 2);
5625         final double mzx = ma.getElementAt(2, 0);
5626         final double mzy = ma.getElementAt(2, 1);
5627 
5628         final Acceleration bx = new Acceleration(biasX,
5629                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5630         final Acceleration by = new Acceleration(biasY,
5631                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5632         final Acceleration bz = new Acceleration(biasZ,
5633                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5634 
5635         final KnownPositionAccelerometerCalibrator calibrator =
5636                 new KnownPositionAccelerometerCalibrator(true,
5637                         bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
5638 
5639         // check default values
5640         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5641         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5642         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5643         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5644         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5645         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5646         final Acceleration bx2 = new Acceleration(0.0,
5647                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5648         calibrator.getInitialBiasXAsAcceleration(bx2);
5649         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5650         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5651         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5652         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5653         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5654         final Acceleration by2 = new Acceleration(0.0,
5655                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5656         calibrator.getInitialBiasYAsAcceleration(by2);
5657         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5658         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5659         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5660         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5661         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5662         final Acceleration bz2 = new Acceleration(0.0,
5663                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5664         calibrator.getInitialBiasZAsAcceleration(bz2);
5665         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5666         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5667         assertEquals(calibrator.getInitialSx(), sx, 0.0);
5668         assertEquals(calibrator.getInitialSy(), sy, 0.0);
5669         assertEquals(calibrator.getInitialSz(), sz, 0.0);
5670         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5671         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5672         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5673         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5674         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5675         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5676         final double[] bias1 = calibrator.getInitialBias();
5677         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5678         final double[] bias2 = new double[3];
5679         calibrator.getInitialBias(bias2);
5680         assertArrayEquals(bias1, bias2, 0.0);
5681         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5682         assertEquals(b1, ba);
5683         final Matrix b2 = new Matrix(3, 1);
5684         calibrator.getInitialBiasAsMatrix(b2);
5685         assertEquals(b1, b2);
5686         final Matrix ma1 = new Matrix(3, 3);
5687         ma1.setSubmatrix(0, 0,
5688                 2, 2,
5689                 new double[]{sx, myx, mzx,
5690                         mxy, sy, mzy,
5691                         mxz, myz, sz});
5692         assertEquals(calibrator.getInitialMa(), ma1);
5693         final Matrix ma2 = new Matrix(3, 3);
5694         calibrator.getInitialMa(ma2);
5695         assertEquals(ma1, ma2);
5696         assertNull(calibrator.getEcefPosition());
5697         assertNull(calibrator.getNedPosition());
5698         assertFalse(calibrator.getNedPosition(null));
5699         assertNull(calibrator.getMeasurements());
5700         assertTrue(calibrator.isCommonAxisUsed());
5701         assertNull(calibrator.getListener());
5702         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
5703         assertFalse(calibrator.isReady());
5704         assertFalse(calibrator.isRunning());
5705         assertNull(calibrator.getEstimatedBiases());
5706         assertFalse(calibrator.getEstimatedBiases(null));
5707         assertNull(calibrator.getEstimatedBiasesAsMatrix());
5708         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5709         assertNull(calibrator.getEstimatedBiasFx());
5710         assertNull(calibrator.getEstimatedBiasFy());
5711         assertNull(calibrator.getEstimatedBiasFz());
5712         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5713         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5714         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5715         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5716         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5717         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5718         assertNull(calibrator.getEstimatedMa());
5719         assertNull(calibrator.getEstimatedSx());
5720         assertNull(calibrator.getEstimatedSy());
5721         assertNull(calibrator.getEstimatedSz());
5722         assertNull(calibrator.getEstimatedMxy());
5723         assertNull(calibrator.getEstimatedMxz());
5724         assertNull(calibrator.getEstimatedMyx());
5725         assertNull(calibrator.getEstimatedMyz());
5726         assertNull(calibrator.getEstimatedMzx());
5727         assertNull(calibrator.getEstimatedMzy());
5728         assertNull(calibrator.getEstimatedCovariance());
5729         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5730         assertNull(calibrator.getGroundTruthGravityNorm());
5731         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5732         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5733     }
5734 
5735     @Test
5736     public void testConstructor52() throws WrongSizeException {
5737         final Matrix ba = generateBa();
5738         final double biasX = ba.getElementAtIndex(0);
5739         final double biasY = ba.getElementAtIndex(1);
5740         final double biasZ = ba.getElementAtIndex(2);
5741 
5742         final Matrix ma = generateMaCommonAxis();
5743         final double sx = ma.getElementAt(0, 0);
5744         final double sy = ma.getElementAt(1, 1);
5745         final double sz = ma.getElementAt(2, 2);
5746         final double mxy = ma.getElementAt(0, 1);
5747         final double mxz = ma.getElementAt(0, 2);
5748         final double myx = ma.getElementAt(1, 0);
5749         final double myz = ma.getElementAt(1, 2);
5750         final double mzx = ma.getElementAt(2, 0);
5751         final double mzy = ma.getElementAt(2, 1);
5752 
5753         final Acceleration bx = new Acceleration(biasX,
5754                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5755         final Acceleration by = new Acceleration(biasY,
5756                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5757         final Acceleration bz = new Acceleration(biasZ,
5758                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5759 
5760         final KnownPositionAccelerometerCalibrator calibrator =
5761                 new KnownPositionAccelerometerCalibrator(true,
5762                         bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy,
5763                         this);
5764 
5765         // check default values
5766         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5767         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5768         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5769         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5770         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5771         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5772         final Acceleration bx2 = new Acceleration(0.0,
5773                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5774         calibrator.getInitialBiasXAsAcceleration(bx2);
5775         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5776         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5777         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5778         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5779         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5780         final Acceleration by2 = new Acceleration(0.0,
5781                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5782         calibrator.getInitialBiasYAsAcceleration(by2);
5783         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5784         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5785         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5786         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5787         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5788         final Acceleration bz2 = new Acceleration(0.0,
5789                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5790         calibrator.getInitialBiasZAsAcceleration(bz2);
5791         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5792         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5793         assertEquals(calibrator.getInitialSx(), sx, 0.0);
5794         assertEquals(calibrator.getInitialSy(), sy, 0.0);
5795         assertEquals(calibrator.getInitialSz(), sz, 0.0);
5796         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5797         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5798         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5799         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5800         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5801         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5802         final double[] bias1 = calibrator.getInitialBias();
5803         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5804         final double[] bias2 = new double[3];
5805         calibrator.getInitialBias(bias2);
5806         assertArrayEquals(bias1, bias2, 0.0);
5807         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5808         assertEquals(b1, ba);
5809         final Matrix b2 = new Matrix(3, 1);
5810         calibrator.getInitialBiasAsMatrix(b2);
5811         assertEquals(b1, b2);
5812         final Matrix ma1 = new Matrix(3, 3);
5813         ma1.setSubmatrix(0, 0,
5814                 2, 2,
5815                 new double[]{sx, myx, mzx,
5816                         mxy, sy, mzy,
5817                         mxz, myz, sz});
5818         assertEquals(calibrator.getInitialMa(), ma1);
5819         final Matrix ma2 = new Matrix(3, 3);
5820         calibrator.getInitialMa(ma2);
5821         assertEquals(ma1, ma2);
5822         assertNull(calibrator.getEcefPosition());
5823         assertNull(calibrator.getNedPosition());
5824         assertFalse(calibrator.getNedPosition(null));
5825         assertNull(calibrator.getMeasurements());
5826         assertTrue(calibrator.isCommonAxisUsed());
5827         assertSame(calibrator.getListener(), this);
5828         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
5829         assertFalse(calibrator.isReady());
5830         assertFalse(calibrator.isRunning());
5831         assertNull(calibrator.getEstimatedBiases());
5832         assertFalse(calibrator.getEstimatedBiases(null));
5833         assertNull(calibrator.getEstimatedBiasesAsMatrix());
5834         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5835         assertNull(calibrator.getEstimatedBiasFx());
5836         assertNull(calibrator.getEstimatedBiasFy());
5837         assertNull(calibrator.getEstimatedBiasFz());
5838         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5839         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5840         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5841         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5842         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5843         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5844         assertNull(calibrator.getEstimatedMa());
5845         assertNull(calibrator.getEstimatedSx());
5846         assertNull(calibrator.getEstimatedSy());
5847         assertNull(calibrator.getEstimatedSz());
5848         assertNull(calibrator.getEstimatedMxy());
5849         assertNull(calibrator.getEstimatedMxz());
5850         assertNull(calibrator.getEstimatedMyx());
5851         assertNull(calibrator.getEstimatedMyz());
5852         assertNull(calibrator.getEstimatedMzx());
5853         assertNull(calibrator.getEstimatedMzy());
5854         assertNull(calibrator.getEstimatedCovariance());
5855         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5856         assertNull(calibrator.getGroundTruthGravityNorm());
5857         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5858         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5859     }
5860 
5861     @Test
5862     public void testConstructor53() throws WrongSizeException {
5863         final Collection<StandardDeviationBodyKinematics> measurements =
5864                 Collections.emptyList();
5865 
5866         final Matrix ba = generateBa();
5867         final double biasX = ba.getElementAtIndex(0);
5868         final double biasY = ba.getElementAtIndex(1);
5869         final double biasZ = ba.getElementAtIndex(2);
5870 
5871         final Matrix ma = generateMaCommonAxis();
5872         final double sx = ma.getElementAt(0, 0);
5873         final double sy = ma.getElementAt(1, 1);
5874         final double sz = ma.getElementAt(2, 2);
5875         final double mxy = ma.getElementAt(0, 1);
5876         final double mxz = ma.getElementAt(0, 2);
5877         final double myx = ma.getElementAt(1, 0);
5878         final double myz = ma.getElementAt(1, 2);
5879         final double mzx = ma.getElementAt(2, 0);
5880         final double mzy = ma.getElementAt(2, 1);
5881 
5882         final Acceleration bx = new Acceleration(biasX,
5883                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5884         final Acceleration by = new Acceleration(biasY,
5885                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5886         final Acceleration bz = new Acceleration(biasZ,
5887                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5888 
5889         final KnownPositionAccelerometerCalibrator calibrator =
5890                 new KnownPositionAccelerometerCalibrator(measurements,
5891                         true, bx, by, bz, sx, sy, sz,
5892                         mxy, mxz, myx, myz, mzx, mzy);
5893 
5894         // check default values
5895         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5896         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5897         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5898         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5899         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5900         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5901         final Acceleration bx2 = new Acceleration(0.0,
5902                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5903         calibrator.getInitialBiasXAsAcceleration(bx2);
5904         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5905         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5906         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5907         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5908         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5909         final Acceleration by2 = new Acceleration(0.0,
5910                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5911         calibrator.getInitialBiasYAsAcceleration(by2);
5912         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5913         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5914         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5915         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5916         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5917         final Acceleration bz2 = new Acceleration(0.0,
5918                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5919         calibrator.getInitialBiasZAsAcceleration(bz2);
5920         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5921         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5922         assertEquals(calibrator.getInitialSx(), sx, 0.0);
5923         assertEquals(calibrator.getInitialSy(), sy, 0.0);
5924         assertEquals(calibrator.getInitialSz(), sz, 0.0);
5925         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5926         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5927         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5928         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5929         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5930         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5931         final double[] bias1 = calibrator.getInitialBias();
5932         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5933         final double[] bias2 = new double[3];
5934         calibrator.getInitialBias(bias2);
5935         assertArrayEquals(bias1, bias2, 0.0);
5936         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5937         assertEquals(b1, ba);
5938         final Matrix b2 = new Matrix(3, 1);
5939         calibrator.getInitialBiasAsMatrix(b2);
5940         assertEquals(b1, b2);
5941         final Matrix ma1 = new Matrix(3, 3);
5942         ma1.setSubmatrix(0, 0,
5943                 2, 2,
5944                 new double[]{sx, myx, mzx,
5945                         mxy, sy, mzy,
5946                         mxz, myz, sz});
5947         assertEquals(calibrator.getInitialMa(), ma1);
5948         final Matrix ma2 = new Matrix(3, 3);
5949         calibrator.getInitialMa(ma2);
5950         assertEquals(ma1, ma2);
5951         assertNull(calibrator.getEcefPosition());
5952         assertNull(calibrator.getNedPosition());
5953         assertFalse(calibrator.getNedPosition(null));
5954         assertSame(calibrator.getMeasurements(), measurements);
5955         assertTrue(calibrator.isCommonAxisUsed());
5956         assertNull(calibrator.getListener());
5957         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
5958         assertFalse(calibrator.isReady());
5959         assertFalse(calibrator.isRunning());
5960         assertNull(calibrator.getEstimatedBiases());
5961         assertFalse(calibrator.getEstimatedBiases(null));
5962         assertNull(calibrator.getEstimatedBiasesAsMatrix());
5963         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5964         assertNull(calibrator.getEstimatedBiasFx());
5965         assertNull(calibrator.getEstimatedBiasFy());
5966         assertNull(calibrator.getEstimatedBiasFz());
5967         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5968         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5969         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5970         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5971         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5972         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5973         assertNull(calibrator.getEstimatedMa());
5974         assertNull(calibrator.getEstimatedSx());
5975         assertNull(calibrator.getEstimatedSy());
5976         assertNull(calibrator.getEstimatedSz());
5977         assertNull(calibrator.getEstimatedMxy());
5978         assertNull(calibrator.getEstimatedMxz());
5979         assertNull(calibrator.getEstimatedMyx());
5980         assertNull(calibrator.getEstimatedMyz());
5981         assertNull(calibrator.getEstimatedMzx());
5982         assertNull(calibrator.getEstimatedMzy());
5983         assertNull(calibrator.getEstimatedCovariance());
5984         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5985         assertNull(calibrator.getGroundTruthGravityNorm());
5986         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5987         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5988     }
5989 
5990     @Test
5991     public void testConstructor54() throws WrongSizeException {
5992         final Collection<StandardDeviationBodyKinematics> measurements =
5993                 Collections.emptyList();
5994 
5995         final Matrix ba = generateBa();
5996         final double biasX = ba.getElementAtIndex(0);
5997         final double biasY = ba.getElementAtIndex(1);
5998         final double biasZ = ba.getElementAtIndex(2);
5999 
6000         final Matrix ma = generateMaCommonAxis();
6001         final double sx = ma.getElementAt(0, 0);
6002         final double sy = ma.getElementAt(1, 1);
6003         final double sz = ma.getElementAt(2, 2);
6004         final double mxy = ma.getElementAt(0, 1);
6005         final double mxz = ma.getElementAt(0, 2);
6006         final double myx = ma.getElementAt(1, 0);
6007         final double myz = ma.getElementAt(1, 2);
6008         final double mzx = ma.getElementAt(2, 0);
6009         final double mzy = ma.getElementAt(2, 1);
6010 
6011         final Acceleration bx = new Acceleration(biasX,
6012                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
6013         final Acceleration by = new Acceleration(biasY,
6014                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
6015         final Acceleration bz = new Acceleration(biasZ,
6016                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
6017 
6018         final KnownPositionAccelerometerCalibrator calibrator =
6019                 new KnownPositionAccelerometerCalibrator(measurements,
6020                         true, bx, by, bz, sx, sy, sz,
6021                         mxy, mxz, myx, myz, mzx, mzy, this);
6022 
6023         // check default values
6024         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6025         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6026         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6027         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6028         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6029         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6030         final Acceleration bx2 = new Acceleration(0.0,
6031                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6032         calibrator.getInitialBiasXAsAcceleration(bx2);
6033         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6034         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6035         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6036         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6037         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6038         final Acceleration by2 = new Acceleration(0.0,
6039                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6040         calibrator.getInitialBiasYAsAcceleration(by2);
6041         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6042         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6043         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6044         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6045         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6046         final Acceleration bz2 = new Acceleration(0.0,
6047                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6048         calibrator.getInitialBiasZAsAcceleration(bz2);
6049         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6050         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6051         assertEquals(calibrator.getInitialSx(), sx, 0.0);
6052         assertEquals(calibrator.getInitialSy(), sy, 0.0);
6053         assertEquals(calibrator.getInitialSz(), sz, 0.0);
6054         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
6055         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
6056         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
6057         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
6058         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
6059         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
6060         final double[] bias1 = calibrator.getInitialBias();
6061         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6062         final double[] bias2 = new double[3];
6063         calibrator.getInitialBias(bias2);
6064         assertArrayEquals(bias1, bias2, 0.0);
6065         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6066         assertEquals(b1, ba);
6067         final Matrix b2 = new Matrix(3, 1);
6068         calibrator.getInitialBiasAsMatrix(b2);
6069         assertEquals(b1, b2);
6070         final Matrix ma1 = new Matrix(3, 3);
6071         ma1.setSubmatrix(0, 0,
6072                 2, 2,
6073                 new double[]{sx, myx, mzx,
6074                         mxy, sy, mzy,
6075                         mxz, myz, sz});
6076         assertEquals(calibrator.getInitialMa(), ma1);
6077         final Matrix ma2 = new Matrix(3, 3);
6078         calibrator.getInitialMa(ma2);
6079         assertEquals(ma1, ma2);
6080         assertNull(calibrator.getEcefPosition());
6081         assertNull(calibrator.getNedPosition());
6082         assertFalse(calibrator.getNedPosition(null));
6083         assertSame(calibrator.getMeasurements(), measurements);
6084         assertTrue(calibrator.isCommonAxisUsed());
6085         assertSame(calibrator.getListener(), this);
6086         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6087         assertFalse(calibrator.isReady());
6088         assertFalse(calibrator.isRunning());
6089         assertNull(calibrator.getEstimatedBiases());
6090         assertFalse(calibrator.getEstimatedBiases(null));
6091         assertNull(calibrator.getEstimatedBiasesAsMatrix());
6092         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6093         assertNull(calibrator.getEstimatedBiasFx());
6094         assertNull(calibrator.getEstimatedBiasFy());
6095         assertNull(calibrator.getEstimatedBiasFz());
6096         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6097         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6098         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6099         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6100         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6101         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6102         assertNull(calibrator.getEstimatedMa());
6103         assertNull(calibrator.getEstimatedSx());
6104         assertNull(calibrator.getEstimatedSy());
6105         assertNull(calibrator.getEstimatedSz());
6106         assertNull(calibrator.getEstimatedMxy());
6107         assertNull(calibrator.getEstimatedMxz());
6108         assertNull(calibrator.getEstimatedMyx());
6109         assertNull(calibrator.getEstimatedMyz());
6110         assertNull(calibrator.getEstimatedMzx());
6111         assertNull(calibrator.getEstimatedMzy());
6112         assertNull(calibrator.getEstimatedCovariance());
6113         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6114         assertNull(calibrator.getGroundTruthGravityNorm());
6115         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6116         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6117     }
6118 
6119     @Test
6120     public void testConstructor55() throws WrongSizeException {
6121         final Matrix ba = generateBa();
6122         final double[] bias = ba.getBuffer();
6123         final double biasX = ba.getElementAtIndex(0);
6124         final double biasY = ba.getElementAtIndex(1);
6125         final double biasZ = ba.getElementAtIndex(2);
6126 
6127         KnownPositionAccelerometerCalibrator calibrator =
6128                 new KnownPositionAccelerometerCalibrator(bias);
6129 
6130         // check default values
6131         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6132         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6133         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6134         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6135         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6136         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6137         final Acceleration bx2 = new Acceleration(0.0,
6138                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6139         calibrator.getInitialBiasXAsAcceleration(bx2);
6140         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6141         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6142         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6143         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6144         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6145         final Acceleration by2 = new Acceleration(0.0,
6146                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6147         calibrator.getInitialBiasYAsAcceleration(by2);
6148         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6149         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6150         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6151         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6152         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6153         final Acceleration bz2 = new Acceleration(0.0,
6154                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6155         calibrator.getInitialBiasZAsAcceleration(bz2);
6156         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6157         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6158         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6159         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6160         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6161         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6162         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6163         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6164         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6165         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6166         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6167         final double[] bias1 = calibrator.getInitialBias();
6168         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6169         final double[] bias2 = new double[3];
6170         calibrator.getInitialBias(bias2);
6171         assertArrayEquals(bias1, bias2, 0.0);
6172         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6173         assertEquals(b1, ba);
6174         final Matrix b2 = new Matrix(3, 1);
6175         calibrator.getInitialBiasAsMatrix(b2);
6176         assertEquals(b1, b2);
6177         final Matrix ma1 = calibrator.getInitialMa();
6178         assertEquals(ma1, new Matrix(3, 3));
6179         final Matrix ma2 = new Matrix(3, 3);
6180         calibrator.getInitialMa(ma2);
6181         assertEquals(ma1, ma2);
6182         assertNull(calibrator.getEcefPosition());
6183         assertNull(calibrator.getNedPosition());
6184         assertFalse(calibrator.getNedPosition(null));
6185         assertNull(calibrator.getMeasurements());
6186         assertFalse(calibrator.isCommonAxisUsed());
6187         assertNull(calibrator.getListener());
6188         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
6189         assertFalse(calibrator.isReady());
6190         assertFalse(calibrator.isRunning());
6191         assertNull(calibrator.getEstimatedBiases());
6192         assertFalse(calibrator.getEstimatedBiases(null));
6193         assertNull(calibrator.getEstimatedBiasesAsMatrix());
6194         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6195         assertNull(calibrator.getEstimatedBiasFx());
6196         assertNull(calibrator.getEstimatedBiasFy());
6197         assertNull(calibrator.getEstimatedBiasFz());
6198         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6199         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6200         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6201         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6202         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6203         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6204         assertNull(calibrator.getEstimatedMa());
6205         assertNull(calibrator.getEstimatedSx());
6206         assertNull(calibrator.getEstimatedSy());
6207         assertNull(calibrator.getEstimatedSz());
6208         assertNull(calibrator.getEstimatedMxy());
6209         assertNull(calibrator.getEstimatedMxz());
6210         assertNull(calibrator.getEstimatedMyx());
6211         assertNull(calibrator.getEstimatedMyz());
6212         assertNull(calibrator.getEstimatedMzx());
6213         assertNull(calibrator.getEstimatedMzy());
6214         assertNull(calibrator.getEstimatedCovariance());
6215         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6216         assertNull(calibrator.getGroundTruthGravityNorm());
6217         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6218         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6219 
6220         // Force IllegalArgumentException
6221         calibrator = null;
6222         try {
6223             calibrator = new KnownPositionAccelerometerCalibrator(new double[1]);
6224             fail("IllegalArgumentException expected but not thrown");
6225         } catch (final IllegalArgumentException ignore) {
6226         }
6227         assertNull(calibrator);
6228     }
6229 
6230     @Test
6231     public void testConstructor56() throws WrongSizeException {
6232         final Matrix ba = generateBa();
6233         final double[] bias = ba.getBuffer();
6234         final double biasX = ba.getElementAtIndex(0);
6235         final double biasY = ba.getElementAtIndex(1);
6236         final double biasZ = ba.getElementAtIndex(2);
6237 
6238         KnownPositionAccelerometerCalibrator calibrator =
6239                 new KnownPositionAccelerometerCalibrator(bias, this);
6240 
6241         // check default values
6242         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6243         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6244         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6245         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6246         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6247         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6248         final Acceleration bx2 = new Acceleration(0.0,
6249                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6250         calibrator.getInitialBiasXAsAcceleration(bx2);
6251         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6252         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6253         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6254         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6255         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6256         final Acceleration by2 = new Acceleration(0.0,
6257                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6258         calibrator.getInitialBiasYAsAcceleration(by2);
6259         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6260         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6261         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6262         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6263         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6264         final Acceleration bz2 = new Acceleration(0.0,
6265                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6266         calibrator.getInitialBiasZAsAcceleration(bz2);
6267         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6268         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6269         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6270         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6271         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6272         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6273         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6274         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6275         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6276         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6277         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6278         final double[] bias1 = calibrator.getInitialBias();
6279         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6280         final double[] bias2 = new double[3];
6281         calibrator.getInitialBias(bias2);
6282         assertArrayEquals(bias1, bias2, 0.0);
6283         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6284         assertEquals(b1, ba);
6285         final Matrix b2 = new Matrix(3, 1);
6286         calibrator.getInitialBiasAsMatrix(b2);
6287         assertEquals(b1, b2);
6288         final Matrix ma1 = calibrator.getInitialMa();
6289         assertEquals(ma1, new Matrix(3, 3));
6290         final Matrix ma2 = new Matrix(3, 3);
6291         calibrator.getInitialMa(ma2);
6292         assertEquals(ma1, ma2);
6293         assertNull(calibrator.getEcefPosition());
6294         assertNull(calibrator.getNedPosition());
6295         assertFalse(calibrator.getNedPosition(null));
6296         assertNull(calibrator.getMeasurements());
6297         assertFalse(calibrator.isCommonAxisUsed());
6298         assertSame(calibrator.getListener(), this);
6299         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
6300         assertFalse(calibrator.isReady());
6301         assertFalse(calibrator.isRunning());
6302         assertNull(calibrator.getEstimatedBiases());
6303         assertFalse(calibrator.getEstimatedBiases(null));
6304         assertNull(calibrator.getEstimatedBiasesAsMatrix());
6305         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6306         assertNull(calibrator.getEstimatedBiasFx());
6307         assertNull(calibrator.getEstimatedBiasFy());
6308         assertNull(calibrator.getEstimatedBiasFz());
6309         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6310         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6311         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6312         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6313         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6314         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6315         assertNull(calibrator.getEstimatedMa());
6316         assertNull(calibrator.getEstimatedSx());
6317         assertNull(calibrator.getEstimatedSy());
6318         assertNull(calibrator.getEstimatedSz());
6319         assertNull(calibrator.getEstimatedMxy());
6320         assertNull(calibrator.getEstimatedMxz());
6321         assertNull(calibrator.getEstimatedMyx());
6322         assertNull(calibrator.getEstimatedMyz());
6323         assertNull(calibrator.getEstimatedMzx());
6324         assertNull(calibrator.getEstimatedMzy());
6325         assertNull(calibrator.getEstimatedCovariance());
6326         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6327         assertNull(calibrator.getGroundTruthGravityNorm());
6328         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6329         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6330 
6331         // Force IllegalArgumentException
6332         calibrator = null;
6333         try {
6334             calibrator = new KnownPositionAccelerometerCalibrator(new double[1],
6335                     this);
6336             fail("IllegalArgumentException expected but not thrown");
6337         } catch (final IllegalArgumentException ignore) {
6338         }
6339         assertNull(calibrator);
6340     }
6341 
6342     @Test
6343     public void testConstructor57() throws WrongSizeException {
6344         final Collection<StandardDeviationBodyKinematics> measurements =
6345                 Collections.emptyList();
6346 
6347         final Matrix ba = generateBa();
6348         final double[] bias = ba.getBuffer();
6349         final double biasX = ba.getElementAtIndex(0);
6350         final double biasY = ba.getElementAtIndex(1);
6351         final double biasZ = ba.getElementAtIndex(2);
6352 
6353         KnownPositionAccelerometerCalibrator calibrator =
6354                 new KnownPositionAccelerometerCalibrator(measurements, bias);
6355 
6356         // check default values
6357         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6358         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6359         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6360         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6361         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6362         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6363         final Acceleration bx2 = new Acceleration(0.0,
6364                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6365         calibrator.getInitialBiasXAsAcceleration(bx2);
6366         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6367         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6368         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6369         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6370         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6371         final Acceleration by2 = new Acceleration(0.0,
6372                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6373         calibrator.getInitialBiasYAsAcceleration(by2);
6374         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6375         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6376         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6377         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6378         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6379         final Acceleration bz2 = new Acceleration(0.0,
6380                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6381         calibrator.getInitialBiasZAsAcceleration(bz2);
6382         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6383         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6384         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6385         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6386         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6387         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6388         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6389         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6390         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6391         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6392         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6393         final double[] bias1 = calibrator.getInitialBias();
6394         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6395         final double[] bias2 = new double[3];
6396         calibrator.getInitialBias(bias2);
6397         assertArrayEquals(bias1, bias2, 0.0);
6398         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6399         assertEquals(b1, ba);
6400         final Matrix b2 = new Matrix(3, 1);
6401         calibrator.getInitialBiasAsMatrix(b2);
6402         assertEquals(b1, b2);
6403         final Matrix ma1 = calibrator.getInitialMa();
6404         assertEquals(ma1, new Matrix(3, 3));
6405         final Matrix ma2 = new Matrix(3, 3);
6406         calibrator.getInitialMa(ma2);
6407         assertEquals(ma1, ma2);
6408         assertNull(calibrator.getEcefPosition());
6409         assertNull(calibrator.getNedPosition());
6410         assertFalse(calibrator.getNedPosition(null));
6411         assertSame(calibrator.getMeasurements(), measurements);
6412         assertFalse(calibrator.isCommonAxisUsed());
6413         assertNull(calibrator.getListener());
6414         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
6415         assertFalse(calibrator.isReady());
6416         assertFalse(calibrator.isRunning());
6417         assertNull(calibrator.getEstimatedBiases());
6418         assertFalse(calibrator.getEstimatedBiases(null));
6419         assertNull(calibrator.getEstimatedBiasesAsMatrix());
6420         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6421         assertNull(calibrator.getEstimatedBiasFx());
6422         assertNull(calibrator.getEstimatedBiasFy());
6423         assertNull(calibrator.getEstimatedBiasFz());
6424         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6425         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6426         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6427         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6428         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6429         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6430         assertNull(calibrator.getEstimatedMa());
6431         assertNull(calibrator.getEstimatedSx());
6432         assertNull(calibrator.getEstimatedSy());
6433         assertNull(calibrator.getEstimatedSz());
6434         assertNull(calibrator.getEstimatedMxy());
6435         assertNull(calibrator.getEstimatedMxz());
6436         assertNull(calibrator.getEstimatedMyx());
6437         assertNull(calibrator.getEstimatedMyz());
6438         assertNull(calibrator.getEstimatedMzx());
6439         assertNull(calibrator.getEstimatedMzy());
6440         assertNull(calibrator.getEstimatedCovariance());
6441         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6442         assertNull(calibrator.getGroundTruthGravityNorm());
6443         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6444         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6445 
6446         // Force IllegalArgumentException
6447         calibrator = null;
6448         try {
6449             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
6450                     new double[1]);
6451             fail("IllegalArgumentException expected but not thrown");
6452         } catch (final IllegalArgumentException ignore) {
6453         }
6454         assertNull(calibrator);
6455     }
6456 
6457     @Test
6458     public void testConstructor58() throws WrongSizeException {
6459         final Collection<StandardDeviationBodyKinematics> measurements =
6460                 Collections.emptyList();
6461 
6462         final Matrix ba = generateBa();
6463         final double[] bias = ba.getBuffer();
6464         final double biasX = ba.getElementAtIndex(0);
6465         final double biasY = ba.getElementAtIndex(1);
6466         final double biasZ = ba.getElementAtIndex(2);
6467 
6468         KnownPositionAccelerometerCalibrator calibrator =
6469                 new KnownPositionAccelerometerCalibrator(measurements, bias,
6470                         this);
6471 
6472         // check default values
6473         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6474         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6475         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6476         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6477         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6478         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6479         final Acceleration bx2 = new Acceleration(0.0,
6480                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6481         calibrator.getInitialBiasXAsAcceleration(bx2);
6482         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6483         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6484         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6485         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6486         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6487         final Acceleration by2 = new Acceleration(0.0,
6488                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6489         calibrator.getInitialBiasYAsAcceleration(by2);
6490         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6491         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6492         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6493         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6494         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6495         final Acceleration bz2 = new Acceleration(0.0,
6496                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6497         calibrator.getInitialBiasZAsAcceleration(bz2);
6498         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6499         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6500         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6501         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6502         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6503         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6504         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6505         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6506         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6507         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6508         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6509         final double[] bias1 = calibrator.getInitialBias();
6510         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6511         final double[] bias2 = new double[3];
6512         calibrator.getInitialBias(bias2);
6513         assertArrayEquals(bias1, bias2, 0.0);
6514         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6515         assertEquals(b1, ba);
6516         final Matrix b2 = new Matrix(3, 1);
6517         calibrator.getInitialBiasAsMatrix(b2);
6518         assertEquals(b1, b2);
6519         final Matrix ma1 = calibrator.getInitialMa();
6520         assertEquals(ma1, new Matrix(3, 3));
6521         final Matrix ma2 = new Matrix(3, 3);
6522         calibrator.getInitialMa(ma2);
6523         assertEquals(ma1, ma2);
6524         assertNull(calibrator.getEcefPosition());
6525         assertNull(calibrator.getNedPosition());
6526         assertFalse(calibrator.getNedPosition(null));
6527         assertSame(calibrator.getMeasurements(), measurements);
6528         assertFalse(calibrator.isCommonAxisUsed());
6529         assertSame(calibrator.getListener(), this);
6530         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
6531         assertFalse(calibrator.isReady());
6532         assertFalse(calibrator.isRunning());
6533         assertNull(calibrator.getEstimatedBiases());
6534         assertFalse(calibrator.getEstimatedBiases(null));
6535         assertNull(calibrator.getEstimatedBiasesAsMatrix());
6536         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6537         assertNull(calibrator.getEstimatedBiasFx());
6538         assertNull(calibrator.getEstimatedBiasFy());
6539         assertNull(calibrator.getEstimatedBiasFz());
6540         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6541         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6542         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6543         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6544         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6545         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6546         assertNull(calibrator.getEstimatedMa());
6547         assertNull(calibrator.getEstimatedSx());
6548         assertNull(calibrator.getEstimatedSy());
6549         assertNull(calibrator.getEstimatedSz());
6550         assertNull(calibrator.getEstimatedMxy());
6551         assertNull(calibrator.getEstimatedMxz());
6552         assertNull(calibrator.getEstimatedMyx());
6553         assertNull(calibrator.getEstimatedMyz());
6554         assertNull(calibrator.getEstimatedMzx());
6555         assertNull(calibrator.getEstimatedMzy());
6556         assertNull(calibrator.getEstimatedCovariance());
6557         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6558         assertNull(calibrator.getGroundTruthGravityNorm());
6559         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6560         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6561 
6562         // Force IllegalArgumentException
6563         calibrator = null;
6564         try {
6565             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
6566                     new double[1], this);
6567             fail("IllegalArgumentException expected but not thrown");
6568         } catch (final IllegalArgumentException ignore) {
6569         }
6570         assertNull(calibrator);
6571     }
6572 
6573     @Test
6574     public void testConstructor59() throws WrongSizeException {
6575         final Matrix ba = generateBa();
6576         final double[] bias = ba.getBuffer();
6577         final double biasX = ba.getElementAtIndex(0);
6578         final double biasY = ba.getElementAtIndex(1);
6579         final double biasZ = ba.getElementAtIndex(2);
6580 
6581         KnownPositionAccelerometerCalibrator calibrator =
6582                 new KnownPositionAccelerometerCalibrator(true, bias);
6583 
6584         // check default values
6585         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6586         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6587         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6588         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6589         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6590         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6591         final Acceleration bx2 = new Acceleration(0.0,
6592                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6593         calibrator.getInitialBiasXAsAcceleration(bx2);
6594         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6595         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6596         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6597         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6598         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6599         final Acceleration by2 = new Acceleration(0.0,
6600                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6601         calibrator.getInitialBiasYAsAcceleration(by2);
6602         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6603         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6604         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6605         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6606         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6607         final Acceleration bz2 = new Acceleration(0.0,
6608                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6609         calibrator.getInitialBiasZAsAcceleration(bz2);
6610         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6611         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6612         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6613         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6614         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6615         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6616         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6617         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6618         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6619         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6620         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6621         final double[] bias1 = calibrator.getInitialBias();
6622         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6623         final double[] bias2 = new double[3];
6624         calibrator.getInitialBias(bias2);
6625         assertArrayEquals(bias1, bias2, 0.0);
6626         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6627         assertEquals(b1, ba);
6628         final Matrix b2 = new Matrix(3, 1);
6629         calibrator.getInitialBiasAsMatrix(b2);
6630         assertEquals(b1, b2);
6631         final Matrix ma1 = calibrator.getInitialMa();
6632         assertEquals(ma1, new Matrix(3, 3));
6633         final Matrix ma2 = new Matrix(3, 3);
6634         calibrator.getInitialMa(ma2);
6635         assertEquals(ma1, ma2);
6636         assertNull(calibrator.getEcefPosition());
6637         assertNull(calibrator.getNedPosition());
6638         assertFalse(calibrator.getNedPosition(null));
6639         assertNull(calibrator.getMeasurements());
6640         assertTrue(calibrator.isCommonAxisUsed());
6641         assertNull(calibrator.getListener());
6642         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6643         assertFalse(calibrator.isReady());
6644         assertFalse(calibrator.isRunning());
6645         assertNull(calibrator.getEstimatedBiases());
6646         assertFalse(calibrator.getEstimatedBiases(null));
6647         assertNull(calibrator.getEstimatedBiasesAsMatrix());
6648         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6649         assertNull(calibrator.getEstimatedBiasFx());
6650         assertNull(calibrator.getEstimatedBiasFy());
6651         assertNull(calibrator.getEstimatedBiasFz());
6652         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6653         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6654         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6655         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6656         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6657         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6658         assertNull(calibrator.getEstimatedMa());
6659         assertNull(calibrator.getEstimatedSx());
6660         assertNull(calibrator.getEstimatedSy());
6661         assertNull(calibrator.getEstimatedSz());
6662         assertNull(calibrator.getEstimatedMxy());
6663         assertNull(calibrator.getEstimatedMxz());
6664         assertNull(calibrator.getEstimatedMyx());
6665         assertNull(calibrator.getEstimatedMyz());
6666         assertNull(calibrator.getEstimatedMzx());
6667         assertNull(calibrator.getEstimatedMzy());
6668         assertNull(calibrator.getEstimatedCovariance());
6669         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6670         assertNull(calibrator.getGroundTruthGravityNorm());
6671         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6672         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6673 
6674         // Force IllegalArgumentException
6675         calibrator = null;
6676         try {
6677             calibrator = new KnownPositionAccelerometerCalibrator(true,
6678                     new double[1]);
6679             fail("IllegalArgumentException expected but not thrown");
6680         } catch (final IllegalArgumentException ignore) {
6681         }
6682         assertNull(calibrator);
6683     }
6684 
6685     @Test
6686     public void testConstructor60() throws WrongSizeException {
6687         final Matrix ba = generateBa();
6688         final double[] bias = ba.getBuffer();
6689         final double biasX = ba.getElementAtIndex(0);
6690         final double biasY = ba.getElementAtIndex(1);
6691         final double biasZ = ba.getElementAtIndex(2);
6692 
6693         KnownPositionAccelerometerCalibrator calibrator =
6694                 new KnownPositionAccelerometerCalibrator(true, bias, this);
6695 
6696         // check default values
6697         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6698         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6699         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6700         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6701         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6702         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6703         final Acceleration bx2 = new Acceleration(0.0,
6704                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6705         calibrator.getInitialBiasXAsAcceleration(bx2);
6706         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6707         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6708         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6709         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6710         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6711         final Acceleration by2 = new Acceleration(0.0,
6712                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6713         calibrator.getInitialBiasYAsAcceleration(by2);
6714         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6715         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6716         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6717         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6718         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6719         final Acceleration bz2 = new Acceleration(0.0,
6720                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6721         calibrator.getInitialBiasZAsAcceleration(bz2);
6722         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6723         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6724         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6725         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6726         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6727         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6728         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6729         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6730         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6731         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6732         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6733         final double[] bias1 = calibrator.getInitialBias();
6734         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6735         final double[] bias2 = new double[3];
6736         calibrator.getInitialBias(bias2);
6737         assertArrayEquals(bias1, bias2, 0.0);
6738         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6739         assertEquals(b1, ba);
6740         final Matrix b2 = new Matrix(3, 1);
6741         calibrator.getInitialBiasAsMatrix(b2);
6742         assertEquals(b1, b2);
6743         final Matrix ma1 = calibrator.getInitialMa();
6744         assertEquals(ma1, new Matrix(3, 3));
6745         final Matrix ma2 = new Matrix(3, 3);
6746         calibrator.getInitialMa(ma2);
6747         assertEquals(ma1, ma2);
6748         assertNull(calibrator.getEcefPosition());
6749         assertNull(calibrator.getNedPosition());
6750         assertFalse(calibrator.getNedPosition(null));
6751         assertNull(calibrator.getMeasurements());
6752         assertTrue(calibrator.isCommonAxisUsed());
6753         assertSame(calibrator.getListener(), this);
6754         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6755         assertFalse(calibrator.isReady());
6756         assertFalse(calibrator.isRunning());
6757         assertNull(calibrator.getEstimatedBiases());
6758         assertFalse(calibrator.getEstimatedBiases(null));
6759         assertNull(calibrator.getEstimatedBiasesAsMatrix());
6760         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6761         assertNull(calibrator.getEstimatedBiasFx());
6762         assertNull(calibrator.getEstimatedBiasFy());
6763         assertNull(calibrator.getEstimatedBiasFz());
6764         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6765         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6766         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6767         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6768         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6769         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6770         assertNull(calibrator.getEstimatedMa());
6771         assertNull(calibrator.getEstimatedSx());
6772         assertNull(calibrator.getEstimatedSy());
6773         assertNull(calibrator.getEstimatedSz());
6774         assertNull(calibrator.getEstimatedMxy());
6775         assertNull(calibrator.getEstimatedMxz());
6776         assertNull(calibrator.getEstimatedMyx());
6777         assertNull(calibrator.getEstimatedMyz());
6778         assertNull(calibrator.getEstimatedMzx());
6779         assertNull(calibrator.getEstimatedMzy());
6780         assertNull(calibrator.getEstimatedCovariance());
6781         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6782         assertNull(calibrator.getGroundTruthGravityNorm());
6783         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6784         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6785 
6786         // Force IllegalArgumentException
6787         calibrator = null;
6788         try {
6789             calibrator = new KnownPositionAccelerometerCalibrator(true,
6790                     new double[1], this);
6791             fail("IllegalArgumentException expected but not thrown");
6792         } catch (final IllegalArgumentException ignore) {
6793         }
6794         assertNull(calibrator);
6795     }
6796 
6797     @Test
6798     public void testConstructor61() throws WrongSizeException {
6799         final Collection<StandardDeviationBodyKinematics> measurements =
6800                 Collections.emptyList();
6801 
6802         final Matrix ba = generateBa();
6803         final double[] bias = ba.getBuffer();
6804         final double biasX = ba.getElementAtIndex(0);
6805         final double biasY = ba.getElementAtIndex(1);
6806         final double biasZ = ba.getElementAtIndex(2);
6807 
6808         KnownPositionAccelerometerCalibrator calibrator =
6809                 new KnownPositionAccelerometerCalibrator(measurements,
6810                         true, bias);
6811 
6812         // check default values
6813         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6814         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6815         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6816         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6817         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6818         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6819         final Acceleration bx2 = new Acceleration(0.0,
6820                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6821         calibrator.getInitialBiasXAsAcceleration(bx2);
6822         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6823         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6824         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6825         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6826         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6827         final Acceleration by2 = new Acceleration(0.0,
6828                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6829         calibrator.getInitialBiasYAsAcceleration(by2);
6830         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6831         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6832         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6833         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6834         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6835         final Acceleration bz2 = new Acceleration(0.0,
6836                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6837         calibrator.getInitialBiasZAsAcceleration(bz2);
6838         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6839         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6840         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6841         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6842         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6843         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6844         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6845         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6846         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6847         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6848         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6849         final double[] bias1 = calibrator.getInitialBias();
6850         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6851         final double[] bias2 = new double[3];
6852         calibrator.getInitialBias(bias2);
6853         assertArrayEquals(bias1, bias2, 0.0);
6854         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6855         assertEquals(b1, ba);
6856         final Matrix b2 = new Matrix(3, 1);
6857         calibrator.getInitialBiasAsMatrix(b2);
6858         assertEquals(b1, b2);
6859         final Matrix ma1 = calibrator.getInitialMa();
6860         assertEquals(ma1, new Matrix(3, 3));
6861         final Matrix ma2 = new Matrix(3, 3);
6862         calibrator.getInitialMa(ma2);
6863         assertEquals(ma1, ma2);
6864         assertNull(calibrator.getEcefPosition());
6865         assertNull(calibrator.getNedPosition());
6866         assertFalse(calibrator.getNedPosition(null));
6867         assertSame(calibrator.getMeasurements(), measurements);
6868         assertTrue(calibrator.isCommonAxisUsed());
6869         assertNull(calibrator.getListener());
6870         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6871         assertFalse(calibrator.isReady());
6872         assertFalse(calibrator.isRunning());
6873         assertNull(calibrator.getEstimatedBiases());
6874         assertFalse(calibrator.getEstimatedBiases(null));
6875         assertNull(calibrator.getEstimatedBiasesAsMatrix());
6876         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6877         assertNull(calibrator.getEstimatedBiasFx());
6878         assertNull(calibrator.getEstimatedBiasFy());
6879         assertNull(calibrator.getEstimatedBiasFz());
6880         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6881         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6882         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6883         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6884         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6885         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6886         assertNull(calibrator.getEstimatedMa());
6887         assertNull(calibrator.getEstimatedSx());
6888         assertNull(calibrator.getEstimatedSy());
6889         assertNull(calibrator.getEstimatedSz());
6890         assertNull(calibrator.getEstimatedMxy());
6891         assertNull(calibrator.getEstimatedMxz());
6892         assertNull(calibrator.getEstimatedMyx());
6893         assertNull(calibrator.getEstimatedMyz());
6894         assertNull(calibrator.getEstimatedMzx());
6895         assertNull(calibrator.getEstimatedMzy());
6896         assertNull(calibrator.getEstimatedCovariance());
6897         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6898         assertNull(calibrator.getGroundTruthGravityNorm());
6899         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6900         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6901 
6902         // Force IllegalArgumentException
6903         calibrator = null;
6904         try {
6905             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
6906                     true, new double[1]);
6907             fail("IllegalArgumentException expected but not thrown");
6908         } catch (final IllegalArgumentException ignore) {
6909         }
6910         assertNull(calibrator);
6911     }
6912 
6913     @Test
6914     public void testConstructor62() throws WrongSizeException {
6915         final Collection<StandardDeviationBodyKinematics> measurements =
6916                 Collections.emptyList();
6917 
6918         final Matrix ba = generateBa();
6919         final double[] bias = ba.getBuffer();
6920         final double biasX = ba.getElementAtIndex(0);
6921         final double biasY = ba.getElementAtIndex(1);
6922         final double biasZ = ba.getElementAtIndex(2);
6923 
6924         KnownPositionAccelerometerCalibrator calibrator =
6925                 new KnownPositionAccelerometerCalibrator(measurements,
6926                         true, bias, this);
6927 
6928         // check default values
6929         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6930         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6931         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6932         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6933         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6934         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6935         final Acceleration bx2 = new Acceleration(0.0,
6936                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6937         calibrator.getInitialBiasXAsAcceleration(bx2);
6938         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6939         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6940         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6941         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6942         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6943         final Acceleration by2 = new Acceleration(0.0,
6944                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6945         calibrator.getInitialBiasYAsAcceleration(by2);
6946         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6947         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6948         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6949         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6950         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6951         final Acceleration bz2 = new Acceleration(0.0,
6952                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6953         calibrator.getInitialBiasZAsAcceleration(bz2);
6954         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6955         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6956         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6957         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6958         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6959         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6960         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6961         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6962         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6963         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6964         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6965         final double[] bias1 = calibrator.getInitialBias();
6966         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6967         final double[] bias2 = new double[3];
6968         calibrator.getInitialBias(bias2);
6969         assertArrayEquals(bias1, bias2, 0.0);
6970         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6971         assertEquals(b1, ba);
6972         final Matrix b2 = new Matrix(3, 1);
6973         calibrator.getInitialBiasAsMatrix(b2);
6974         assertEquals(b1, b2);
6975         final Matrix ma1 = calibrator.getInitialMa();
6976         assertEquals(ma1, new Matrix(3, 3));
6977         final Matrix ma2 = new Matrix(3, 3);
6978         calibrator.getInitialMa(ma2);
6979         assertEquals(ma1, ma2);
6980         assertNull(calibrator.getEcefPosition());
6981         assertNull(calibrator.getNedPosition());
6982         assertFalse(calibrator.getNedPosition(null));
6983         assertSame(calibrator.getMeasurements(), measurements);
6984         assertTrue(calibrator.isCommonAxisUsed());
6985         assertSame(calibrator.getListener(), this);
6986         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6987         assertFalse(calibrator.isReady());
6988         assertFalse(calibrator.isRunning());
6989         assertNull(calibrator.getEstimatedBiases());
6990         assertFalse(calibrator.getEstimatedBiases(null));
6991         assertNull(calibrator.getEstimatedBiasesAsMatrix());
6992         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6993         assertNull(calibrator.getEstimatedBiasFx());
6994         assertNull(calibrator.getEstimatedBiasFy());
6995         assertNull(calibrator.getEstimatedBiasFz());
6996         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6997         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6998         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6999         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7000         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7001         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7002         assertNull(calibrator.getEstimatedMa());
7003         assertNull(calibrator.getEstimatedSx());
7004         assertNull(calibrator.getEstimatedSy());
7005         assertNull(calibrator.getEstimatedSz());
7006         assertNull(calibrator.getEstimatedMxy());
7007         assertNull(calibrator.getEstimatedMxz());
7008         assertNull(calibrator.getEstimatedMyx());
7009         assertNull(calibrator.getEstimatedMyz());
7010         assertNull(calibrator.getEstimatedMzx());
7011         assertNull(calibrator.getEstimatedMzy());
7012         assertNull(calibrator.getEstimatedCovariance());
7013         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7014         assertNull(calibrator.getGroundTruthGravityNorm());
7015         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7016         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7017 
7018         // Force IllegalArgumentException
7019         calibrator = null;
7020         try {
7021             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7022                     true, new double[1], this);
7023             fail("IllegalArgumentException expected but not thrown");
7024         } catch (final IllegalArgumentException ignore) {
7025         }
7026         assertNull(calibrator);
7027     }
7028 
7029     @Test
7030     public void testConstructor63() throws WrongSizeException {
7031         final Matrix ba = generateBa();
7032         final double biasX = ba.getElementAtIndex(0);
7033         final double biasY = ba.getElementAtIndex(1);
7034         final double biasZ = ba.getElementAtIndex(2);
7035 
7036         KnownPositionAccelerometerCalibrator calibrator =
7037                 new KnownPositionAccelerometerCalibrator(ba);
7038 
7039         // check default values
7040         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7041         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7042         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7043         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7044         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7045         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7046         final Acceleration bx2 = new Acceleration(0.0,
7047                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7048         calibrator.getInitialBiasXAsAcceleration(bx2);
7049         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7050         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7051         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7052         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7053         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7054         final Acceleration by2 = new Acceleration(0.0,
7055                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7056         calibrator.getInitialBiasYAsAcceleration(by2);
7057         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7058         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7059         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7060         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7061         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7062         final Acceleration bz2 = new Acceleration(0.0,
7063                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7064         calibrator.getInitialBiasZAsAcceleration(bz2);
7065         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7066         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7067         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7068         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7069         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7070         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7071         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7072         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7073         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7074         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7075         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7076         final double[] bias1 = calibrator.getInitialBias();
7077         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7078         final double[] bias2 = new double[3];
7079         calibrator.getInitialBias(bias2);
7080         assertArrayEquals(bias1, bias2, 0.0);
7081         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7082         assertEquals(b1, ba);
7083         final Matrix b2 = new Matrix(3, 1);
7084         calibrator.getInitialBiasAsMatrix(b2);
7085         assertEquals(b1, b2);
7086         final Matrix ma1 = calibrator.getInitialMa();
7087         assertEquals(ma1, new Matrix(3, 3));
7088         final Matrix ma2 = new Matrix(3, 3);
7089         calibrator.getInitialMa(ma2);
7090         assertEquals(ma1, ma2);
7091         assertNull(calibrator.getEcefPosition());
7092         assertNull(calibrator.getNedPosition());
7093         assertFalse(calibrator.getNedPosition(null));
7094         assertNull(calibrator.getMeasurements());
7095         assertFalse(calibrator.isCommonAxisUsed());
7096         assertNull(calibrator.getListener());
7097         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
7098         assertFalse(calibrator.isReady());
7099         assertFalse(calibrator.isRunning());
7100         assertNull(calibrator.getEstimatedBiases());
7101         assertFalse(calibrator.getEstimatedBiases(null));
7102         assertNull(calibrator.getEstimatedBiasesAsMatrix());
7103         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7104         assertNull(calibrator.getEstimatedBiasFx());
7105         assertNull(calibrator.getEstimatedBiasFy());
7106         assertNull(calibrator.getEstimatedBiasFz());
7107         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7108         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7109         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7110         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7111         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7112         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7113         assertNull(calibrator.getEstimatedMa());
7114         assertNull(calibrator.getEstimatedSx());
7115         assertNull(calibrator.getEstimatedSy());
7116         assertNull(calibrator.getEstimatedSz());
7117         assertNull(calibrator.getEstimatedMxy());
7118         assertNull(calibrator.getEstimatedMxz());
7119         assertNull(calibrator.getEstimatedMyx());
7120         assertNull(calibrator.getEstimatedMyz());
7121         assertNull(calibrator.getEstimatedMzx());
7122         assertNull(calibrator.getEstimatedMzy());
7123         assertNull(calibrator.getEstimatedCovariance());
7124         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7125         assertNull(calibrator.getGroundTruthGravityNorm());
7126         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7127         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7128 
7129         // Force IllegalArgumentException
7130         calibrator = null;
7131         try {
7132             calibrator = new KnownPositionAccelerometerCalibrator(
7133                     new Matrix(1, 1));
7134             fail("IllegalArgumentException expected but not thrown");
7135         } catch (final IllegalArgumentException ignore) {
7136         }
7137         try {
7138             calibrator = new KnownPositionAccelerometerCalibrator(
7139                     new Matrix(1, 3));
7140             fail("IllegalArgumentException expected but not thrown");
7141         } catch (final IllegalArgumentException ignore) {
7142         }
7143         assertNull(calibrator);
7144     }
7145 
7146     @Test
7147     public void testConstructor64() throws WrongSizeException {
7148         final Matrix ba = generateBa();
7149         final double biasX = ba.getElementAtIndex(0);
7150         final double biasY = ba.getElementAtIndex(1);
7151         final double biasZ = ba.getElementAtIndex(2);
7152 
7153         KnownPositionAccelerometerCalibrator calibrator =
7154                 new KnownPositionAccelerometerCalibrator(ba, this);
7155 
7156         // check default values
7157         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7158         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7159         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7160         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7161         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7162         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7163         final Acceleration bx2 = new Acceleration(0.0,
7164                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7165         calibrator.getInitialBiasXAsAcceleration(bx2);
7166         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7167         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7168         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7169         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7170         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7171         final Acceleration by2 = new Acceleration(0.0,
7172                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7173         calibrator.getInitialBiasYAsAcceleration(by2);
7174         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7175         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7176         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7177         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7178         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7179         final Acceleration bz2 = new Acceleration(0.0,
7180                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7181         calibrator.getInitialBiasZAsAcceleration(bz2);
7182         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7183         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7184         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7185         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7186         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7187         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7188         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7189         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7190         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7191         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7192         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7193         final double[] bias1 = calibrator.getInitialBias();
7194         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7195         final double[] bias2 = new double[3];
7196         calibrator.getInitialBias(bias2);
7197         assertArrayEquals(bias1, bias2, 0.0);
7198         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7199         assertEquals(b1, ba);
7200         final Matrix b2 = new Matrix(3, 1);
7201         calibrator.getInitialBiasAsMatrix(b2);
7202         assertEquals(b1, b2);
7203         final Matrix ma1 = calibrator.getInitialMa();
7204         assertEquals(ma1, new Matrix(3, 3));
7205         final Matrix ma2 = new Matrix(3, 3);
7206         calibrator.getInitialMa(ma2);
7207         assertEquals(ma1, ma2);
7208         assertNull(calibrator.getEcefPosition());
7209         assertNull(calibrator.getNedPosition());
7210         assertFalse(calibrator.getNedPosition(null));
7211         assertNull(calibrator.getMeasurements());
7212         assertFalse(calibrator.isCommonAxisUsed());
7213         assertSame(calibrator.getListener(), this);
7214         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
7215         assertFalse(calibrator.isReady());
7216         assertFalse(calibrator.isRunning());
7217         assertNull(calibrator.getEstimatedBiases());
7218         assertFalse(calibrator.getEstimatedBiases(null));
7219         assertNull(calibrator.getEstimatedBiasesAsMatrix());
7220         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7221         assertNull(calibrator.getEstimatedBiasFx());
7222         assertNull(calibrator.getEstimatedBiasFy());
7223         assertNull(calibrator.getEstimatedBiasFz());
7224         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7225         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7226         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7227         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7228         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7229         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7230         assertNull(calibrator.getEstimatedMa());
7231         assertNull(calibrator.getEstimatedSx());
7232         assertNull(calibrator.getEstimatedSy());
7233         assertNull(calibrator.getEstimatedSz());
7234         assertNull(calibrator.getEstimatedMxy());
7235         assertNull(calibrator.getEstimatedMxz());
7236         assertNull(calibrator.getEstimatedMyx());
7237         assertNull(calibrator.getEstimatedMyz());
7238         assertNull(calibrator.getEstimatedMzx());
7239         assertNull(calibrator.getEstimatedMzy());
7240         assertNull(calibrator.getEstimatedCovariance());
7241         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7242         assertNull(calibrator.getGroundTruthGravityNorm());
7243         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7244         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7245 
7246         // Force IllegalArgumentException
7247         calibrator = null;
7248         try {
7249             calibrator = new KnownPositionAccelerometerCalibrator(
7250                     new Matrix(1, 1));
7251             fail("IllegalArgumentException expected but not thrown");
7252         } catch (final IllegalArgumentException ignore) {
7253         }
7254         try {
7255             calibrator = new KnownPositionAccelerometerCalibrator(
7256                     new Matrix(1, 3));
7257             fail("IllegalArgumentException expected but not thrown");
7258         } catch (final IllegalArgumentException ignore) {
7259         }
7260         assertNull(calibrator);
7261     }
7262 
7263     @Test
7264     public void testConstructor65() throws WrongSizeException {
7265         final Collection<StandardDeviationBodyKinematics> measurements =
7266                 Collections.emptyList();
7267 
7268         final Matrix ba = generateBa();
7269         final double biasX = ba.getElementAtIndex(0);
7270         final double biasY = ba.getElementAtIndex(1);
7271         final double biasZ = ba.getElementAtIndex(2);
7272 
7273         KnownPositionAccelerometerCalibrator calibrator =
7274                 new KnownPositionAccelerometerCalibrator(measurements, ba);
7275 
7276         // check default values
7277         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7278         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7279         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7280         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7281         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7282         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7283         final Acceleration bx2 = new Acceleration(0.0,
7284                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7285         calibrator.getInitialBiasXAsAcceleration(bx2);
7286         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7287         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7288         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7289         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7290         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7291         final Acceleration by2 = new Acceleration(0.0,
7292                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7293         calibrator.getInitialBiasYAsAcceleration(by2);
7294         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7295         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7296         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7297         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7298         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7299         final Acceleration bz2 = new Acceleration(0.0,
7300                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7301         calibrator.getInitialBiasZAsAcceleration(bz2);
7302         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7303         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7304         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7305         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7306         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7307         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7308         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7309         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7310         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7311         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7312         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7313         final double[] bias1 = calibrator.getInitialBias();
7314         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7315         final double[] bias2 = new double[3];
7316         calibrator.getInitialBias(bias2);
7317         assertArrayEquals(bias1, bias2, 0.0);
7318         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7319         assertEquals(b1, ba);
7320         final Matrix b2 = new Matrix(3, 1);
7321         calibrator.getInitialBiasAsMatrix(b2);
7322         assertEquals(b1, b2);
7323         final Matrix ma1 = calibrator.getInitialMa();
7324         assertEquals(ma1, new Matrix(3, 3));
7325         final Matrix ma2 = new Matrix(3, 3);
7326         calibrator.getInitialMa(ma2);
7327         assertEquals(ma1, ma2);
7328         assertNull(calibrator.getEcefPosition());
7329         assertNull(calibrator.getNedPosition());
7330         assertFalse(calibrator.getNedPosition(null));
7331         assertSame(calibrator.getMeasurements(), measurements);
7332         assertFalse(calibrator.isCommonAxisUsed());
7333         assertNull(calibrator.getListener());
7334         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
7335         assertFalse(calibrator.isReady());
7336         assertFalse(calibrator.isRunning());
7337         assertNull(calibrator.getEstimatedBiases());
7338         assertFalse(calibrator.getEstimatedBiases(null));
7339         assertNull(calibrator.getEstimatedBiasesAsMatrix());
7340         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7341         assertNull(calibrator.getEstimatedBiasFx());
7342         assertNull(calibrator.getEstimatedBiasFy());
7343         assertNull(calibrator.getEstimatedBiasFz());
7344         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7345         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7346         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7347         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7348         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7349         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7350         assertNull(calibrator.getEstimatedMa());
7351         assertNull(calibrator.getEstimatedSx());
7352         assertNull(calibrator.getEstimatedSy());
7353         assertNull(calibrator.getEstimatedSz());
7354         assertNull(calibrator.getEstimatedMxy());
7355         assertNull(calibrator.getEstimatedMxz());
7356         assertNull(calibrator.getEstimatedMyx());
7357         assertNull(calibrator.getEstimatedMyz());
7358         assertNull(calibrator.getEstimatedMzx());
7359         assertNull(calibrator.getEstimatedMzy());
7360         assertNull(calibrator.getEstimatedCovariance());
7361         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7362         assertNull(calibrator.getGroundTruthGravityNorm());
7363         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7364         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7365 
7366         // Force IllegalArgumentException
7367         calibrator = null;
7368         try {
7369             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7370                     new Matrix(1, 1));
7371             fail("IllegalArgumentException expected but not thrown");
7372         } catch (final IllegalArgumentException ignore) {
7373         }
7374         try {
7375             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7376                     new Matrix(1, 3));
7377             fail("IllegalArgumentException expected but not thrown");
7378         } catch (final IllegalArgumentException ignore) {
7379         }
7380         assertNull(calibrator);
7381     }
7382 
7383     @Test
7384     public void testConstructor66() throws WrongSizeException {
7385         final Collection<StandardDeviationBodyKinematics> measurements =
7386                 Collections.emptyList();
7387 
7388         final Matrix ba = generateBa();
7389         final double biasX = ba.getElementAtIndex(0);
7390         final double biasY = ba.getElementAtIndex(1);
7391         final double biasZ = ba.getElementAtIndex(2);
7392 
7393         KnownPositionAccelerometerCalibrator calibrator =
7394                 new KnownPositionAccelerometerCalibrator(measurements, ba,
7395                         this);
7396 
7397         // check default values
7398         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7399         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7400         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7401         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7402         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7403         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7404         final Acceleration bx2 = new Acceleration(0.0,
7405                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7406         calibrator.getInitialBiasXAsAcceleration(bx2);
7407         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7408         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7409         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7410         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7411         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7412         final Acceleration by2 = new Acceleration(0.0,
7413                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7414         calibrator.getInitialBiasYAsAcceleration(by2);
7415         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7416         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7417         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7418         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7419         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7420         final Acceleration bz2 = new Acceleration(0.0,
7421                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7422         calibrator.getInitialBiasZAsAcceleration(bz2);
7423         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7424         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7425         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7426         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7427         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7428         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7429         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7430         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7431         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7432         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7433         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7434         final double[] bias1 = calibrator.getInitialBias();
7435         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7436         final double[] bias2 = new double[3];
7437         calibrator.getInitialBias(bias2);
7438         assertArrayEquals(bias1, bias2, 0.0);
7439         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7440         assertEquals(b1, ba);
7441         final Matrix b2 = new Matrix(3, 1);
7442         calibrator.getInitialBiasAsMatrix(b2);
7443         assertEquals(b1, b2);
7444         final Matrix ma1 = calibrator.getInitialMa();
7445         assertEquals(ma1, new Matrix(3, 3));
7446         final Matrix ma2 = new Matrix(3, 3);
7447         calibrator.getInitialMa(ma2);
7448         assertEquals(ma1, ma2);
7449         assertNull(calibrator.getEcefPosition());
7450         assertNull(calibrator.getNedPosition());
7451         assertFalse(calibrator.getNedPosition(null));
7452         assertSame(calibrator.getMeasurements(), measurements);
7453         assertFalse(calibrator.isCommonAxisUsed());
7454         assertSame(calibrator.getListener(), this);
7455         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
7456         assertFalse(calibrator.isReady());
7457         assertFalse(calibrator.isRunning());
7458         assertNull(calibrator.getEstimatedBiases());
7459         assertFalse(calibrator.getEstimatedBiases(null));
7460         assertNull(calibrator.getEstimatedBiasesAsMatrix());
7461         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7462         assertNull(calibrator.getEstimatedBiasFx());
7463         assertNull(calibrator.getEstimatedBiasFy());
7464         assertNull(calibrator.getEstimatedBiasFz());
7465         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7466         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7467         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7468         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7469         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7470         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7471         assertNull(calibrator.getEstimatedMa());
7472         assertNull(calibrator.getEstimatedSx());
7473         assertNull(calibrator.getEstimatedSy());
7474         assertNull(calibrator.getEstimatedSz());
7475         assertNull(calibrator.getEstimatedMxy());
7476         assertNull(calibrator.getEstimatedMxz());
7477         assertNull(calibrator.getEstimatedMyx());
7478         assertNull(calibrator.getEstimatedMyz());
7479         assertNull(calibrator.getEstimatedMzx());
7480         assertNull(calibrator.getEstimatedMzy());
7481         assertNull(calibrator.getEstimatedCovariance());
7482         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7483         assertNull(calibrator.getGroundTruthGravityNorm());
7484         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7485         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7486 
7487         // Force IllegalArgumentException
7488         calibrator = null;
7489         try {
7490             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7491                     new Matrix(1, 1), this);
7492             fail("IllegalArgumentException expected but not thrown");
7493         } catch (final IllegalArgumentException ignore) {
7494         }
7495         try {
7496             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7497                     new Matrix(1, 3), this);
7498             fail("IllegalArgumentException expected but not thrown");
7499         } catch (final IllegalArgumentException ignore) {
7500         }
7501         assertNull(calibrator);
7502     }
7503 
7504     @Test
7505     public void testConstructor67() throws WrongSizeException {
7506         final Matrix ba = generateBa();
7507         final double biasX = ba.getElementAtIndex(0);
7508         final double biasY = ba.getElementAtIndex(1);
7509         final double biasZ = ba.getElementAtIndex(2);
7510 
7511         KnownPositionAccelerometerCalibrator calibrator =
7512                 new KnownPositionAccelerometerCalibrator(true, ba);
7513 
7514         // check default values
7515         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7516         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7517         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7518         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7519         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7520         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7521         final Acceleration bx2 = new Acceleration(0.0,
7522                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7523         calibrator.getInitialBiasXAsAcceleration(bx2);
7524         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7525         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7526         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7527         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7528         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7529         final Acceleration by2 = new Acceleration(0.0,
7530                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7531         calibrator.getInitialBiasYAsAcceleration(by2);
7532         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7533         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7534         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7535         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7536         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7537         final Acceleration bz2 = new Acceleration(0.0,
7538                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7539         calibrator.getInitialBiasZAsAcceleration(bz2);
7540         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7541         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7542         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7543         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7544         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7545         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7546         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7547         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7548         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7549         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7550         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7551         final double[] bias1 = calibrator.getInitialBias();
7552         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7553         final double[] bias2 = new double[3];
7554         calibrator.getInitialBias(bias2);
7555         assertArrayEquals(bias1, bias2, 0.0);
7556         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7557         assertEquals(b1, ba);
7558         final Matrix b2 = new Matrix(3, 1);
7559         calibrator.getInitialBiasAsMatrix(b2);
7560         assertEquals(b1, b2);
7561         final Matrix ma1 = calibrator.getInitialMa();
7562         assertEquals(ma1, new Matrix(3, 3));
7563         final Matrix ma2 = new Matrix(3, 3);
7564         calibrator.getInitialMa(ma2);
7565         assertEquals(ma1, ma2);
7566         assertNull(calibrator.getEcefPosition());
7567         assertNull(calibrator.getNedPosition());
7568         assertFalse(calibrator.getNedPosition(null));
7569         assertNull(calibrator.getMeasurements());
7570         assertTrue(calibrator.isCommonAxisUsed());
7571         assertNull(calibrator.getListener());
7572         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
7573         assertFalse(calibrator.isReady());
7574         assertFalse(calibrator.isRunning());
7575         assertNull(calibrator.getEstimatedBiases());
7576         assertFalse(calibrator.getEstimatedBiases(null));
7577         assertNull(calibrator.getEstimatedBiasesAsMatrix());
7578         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7579         assertNull(calibrator.getEstimatedBiasFx());
7580         assertNull(calibrator.getEstimatedBiasFy());
7581         assertNull(calibrator.getEstimatedBiasFz());
7582         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7583         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7584         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7585         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7586         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7587         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7588         assertNull(calibrator.getEstimatedMa());
7589         assertNull(calibrator.getEstimatedSx());
7590         assertNull(calibrator.getEstimatedSy());
7591         assertNull(calibrator.getEstimatedSz());
7592         assertNull(calibrator.getEstimatedMxy());
7593         assertNull(calibrator.getEstimatedMxz());
7594         assertNull(calibrator.getEstimatedMyx());
7595         assertNull(calibrator.getEstimatedMyz());
7596         assertNull(calibrator.getEstimatedMzx());
7597         assertNull(calibrator.getEstimatedMzy());
7598         assertNull(calibrator.getEstimatedCovariance());
7599         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7600         assertNull(calibrator.getGroundTruthGravityNorm());
7601         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7602         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7603 
7604         // Force IllegalArgumentException
7605         calibrator = null;
7606         try {
7607             calibrator = new KnownPositionAccelerometerCalibrator(
7608                     true, new Matrix(1, 1));
7609             fail("IllegalArgumentException expected but not thrown");
7610         } catch (final IllegalArgumentException ignore) {
7611         }
7612         try {
7613             calibrator = new KnownPositionAccelerometerCalibrator(
7614                     true, new Matrix(1, 3));
7615             fail("IllegalArgumentException expected but not thrown");
7616         } catch (final IllegalArgumentException ignore) {
7617         }
7618         assertNull(calibrator);
7619     }
7620 
7621     @Test
7622     public void testConstructor68() throws WrongSizeException {
7623         final Matrix ba = generateBa();
7624         final double biasX = ba.getElementAtIndex(0);
7625         final double biasY = ba.getElementAtIndex(1);
7626         final double biasZ = ba.getElementAtIndex(2);
7627 
7628         KnownPositionAccelerometerCalibrator calibrator =
7629                 new KnownPositionAccelerometerCalibrator(true, ba,
7630                         this);
7631 
7632         // check default values
7633         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7634         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7635         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7636         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7637         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7638         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7639         final Acceleration bx2 = new Acceleration(0.0,
7640                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7641         calibrator.getInitialBiasXAsAcceleration(bx2);
7642         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7643         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7644         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7645         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7646         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7647         final Acceleration by2 = new Acceleration(0.0,
7648                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7649         calibrator.getInitialBiasYAsAcceleration(by2);
7650         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7651         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7652         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7653         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7654         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7655         final Acceleration bz2 = new Acceleration(0.0,
7656                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7657         calibrator.getInitialBiasZAsAcceleration(bz2);
7658         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7659         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7660         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7661         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7662         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7663         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7664         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7665         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7666         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7667         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7668         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7669         final double[] bias1 = calibrator.getInitialBias();
7670         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7671         final double[] bias2 = new double[3];
7672         calibrator.getInitialBias(bias2);
7673         assertArrayEquals(bias1, bias2, 0.0);
7674         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7675         assertEquals(b1, ba);
7676         final Matrix b2 = new Matrix(3, 1);
7677         calibrator.getInitialBiasAsMatrix(b2);
7678         assertEquals(b1, b2);
7679         final Matrix ma1 = calibrator.getInitialMa();
7680         assertEquals(ma1, new Matrix(3, 3));
7681         final Matrix ma2 = new Matrix(3, 3);
7682         calibrator.getInitialMa(ma2);
7683         assertEquals(ma1, ma2);
7684         assertNull(calibrator.getEcefPosition());
7685         assertNull(calibrator.getNedPosition());
7686         assertFalse(calibrator.getNedPosition(null));
7687         assertNull(calibrator.getMeasurements());
7688         assertTrue(calibrator.isCommonAxisUsed());
7689         assertSame(calibrator.getListener(), this);
7690         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
7691         assertFalse(calibrator.isReady());
7692         assertFalse(calibrator.isRunning());
7693         assertNull(calibrator.getEstimatedBiases());
7694         assertFalse(calibrator.getEstimatedBiases(null));
7695         assertNull(calibrator.getEstimatedBiasesAsMatrix());
7696         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7697         assertNull(calibrator.getEstimatedBiasFx());
7698         assertNull(calibrator.getEstimatedBiasFy());
7699         assertNull(calibrator.getEstimatedBiasFz());
7700         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7701         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7702         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7703         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7704         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7705         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7706         assertNull(calibrator.getEstimatedMa());
7707         assertNull(calibrator.getEstimatedSx());
7708         assertNull(calibrator.getEstimatedSy());
7709         assertNull(calibrator.getEstimatedSz());
7710         assertNull(calibrator.getEstimatedMxy());
7711         assertNull(calibrator.getEstimatedMxz());
7712         assertNull(calibrator.getEstimatedMyx());
7713         assertNull(calibrator.getEstimatedMyz());
7714         assertNull(calibrator.getEstimatedMzx());
7715         assertNull(calibrator.getEstimatedMzy());
7716         assertNull(calibrator.getEstimatedCovariance());
7717         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7718         assertNull(calibrator.getGroundTruthGravityNorm());
7719         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7720         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7721 
7722         // Force IllegalArgumentException
7723         calibrator = null;
7724         try {
7725             calibrator = new KnownPositionAccelerometerCalibrator(
7726                     true, new Matrix(1, 1),
7727                     this);
7728             fail("IllegalArgumentException expected but not thrown");
7729         } catch (final IllegalArgumentException ignore) {
7730         }
7731         try {
7732             calibrator = new KnownPositionAccelerometerCalibrator(
7733                     true, new Matrix(1, 3),
7734                     this);
7735             fail("IllegalArgumentException expected but not thrown");
7736         } catch (final IllegalArgumentException ignore) {
7737         }
7738         assertNull(calibrator);
7739     }
7740 
7741     @Test
7742     public void testConstructor69() throws WrongSizeException {
7743         final Collection<StandardDeviationBodyKinematics> measurements =
7744                 Collections.emptyList();
7745 
7746         final Matrix ba = generateBa();
7747         final double biasX = ba.getElementAtIndex(0);
7748         final double biasY = ba.getElementAtIndex(1);
7749         final double biasZ = ba.getElementAtIndex(2);
7750 
7751         KnownPositionAccelerometerCalibrator calibrator =
7752                 new KnownPositionAccelerometerCalibrator(measurements,
7753                         true, ba);
7754 
7755         // check default values
7756         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7757         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7758         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7759         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7760         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7761         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7762         final Acceleration bx2 = new Acceleration(0.0,
7763                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7764         calibrator.getInitialBiasXAsAcceleration(bx2);
7765         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7766         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7767         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7768         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7769         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7770         final Acceleration by2 = new Acceleration(0.0,
7771                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7772         calibrator.getInitialBiasYAsAcceleration(by2);
7773         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7774         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7775         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7776         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7777         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7778         final Acceleration bz2 = new Acceleration(0.0,
7779                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7780         calibrator.getInitialBiasZAsAcceleration(bz2);
7781         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7782         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7783         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7784         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7785         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7786         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7787         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7788         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7789         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7790         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7791         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7792         final double[] bias1 = calibrator.getInitialBias();
7793         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7794         final double[] bias2 = new double[3];
7795         calibrator.getInitialBias(bias2);
7796         assertArrayEquals(bias1, bias2, 0.0);
7797         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7798         assertEquals(b1, ba);
7799         final Matrix b2 = new Matrix(3, 1);
7800         calibrator.getInitialBiasAsMatrix(b2);
7801         assertEquals(b1, b2);
7802         final Matrix ma1 = calibrator.getInitialMa();
7803         assertEquals(ma1, new Matrix(3, 3));
7804         final Matrix ma2 = new Matrix(3, 3);
7805         calibrator.getInitialMa(ma2);
7806         assertEquals(ma1, ma2);
7807         assertNull(calibrator.getEcefPosition());
7808         assertNull(calibrator.getNedPosition());
7809         assertFalse(calibrator.getNedPosition(null));
7810         assertSame(calibrator.getMeasurements(), measurements);
7811         assertTrue(calibrator.isCommonAxisUsed());
7812         assertNull(calibrator.getListener());
7813         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
7814         assertFalse(calibrator.isReady());
7815         assertFalse(calibrator.isRunning());
7816         assertNull(calibrator.getEstimatedBiases());
7817         assertFalse(calibrator.getEstimatedBiases(null));
7818         assertNull(calibrator.getEstimatedBiasesAsMatrix());
7819         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7820         assertNull(calibrator.getEstimatedBiasFx());
7821         assertNull(calibrator.getEstimatedBiasFy());
7822         assertNull(calibrator.getEstimatedBiasFz());
7823         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7824         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7825         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7826         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7827         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7828         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7829         assertNull(calibrator.getEstimatedMa());
7830         assertNull(calibrator.getEstimatedSx());
7831         assertNull(calibrator.getEstimatedSy());
7832         assertNull(calibrator.getEstimatedSz());
7833         assertNull(calibrator.getEstimatedMxy());
7834         assertNull(calibrator.getEstimatedMxz());
7835         assertNull(calibrator.getEstimatedMyx());
7836         assertNull(calibrator.getEstimatedMyz());
7837         assertNull(calibrator.getEstimatedMzx());
7838         assertNull(calibrator.getEstimatedMzy());
7839         assertNull(calibrator.getEstimatedCovariance());
7840         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7841         assertNull(calibrator.getGroundTruthGravityNorm());
7842         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7843         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7844 
7845         // Force IllegalArgumentException
7846         calibrator = null;
7847         try {
7848             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7849                     true, new Matrix(1, 1));
7850             fail("IllegalArgumentException expected but not thrown");
7851         } catch (final IllegalArgumentException ignore) {
7852         }
7853         try {
7854             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7855                     true, new Matrix(1, 3));
7856             fail("IllegalArgumentException expected but not thrown");
7857         } catch (final IllegalArgumentException ignore) {
7858         }
7859         assertNull(calibrator);
7860     }
7861 
7862     @Test
7863     public void testConstructor70() throws WrongSizeException {
7864         final Collection<StandardDeviationBodyKinematics> measurements =
7865                 Collections.emptyList();
7866 
7867         final Matrix ba = generateBa();
7868         final double biasX = ba.getElementAtIndex(0);
7869         final double biasY = ba.getElementAtIndex(1);
7870         final double biasZ = ba.getElementAtIndex(2);
7871 
7872         KnownPositionAccelerometerCalibrator calibrator =
7873                 new KnownPositionAccelerometerCalibrator(measurements,
7874                         true, ba, this);
7875 
7876         // check default values
7877         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7878         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7879         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7880         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7881         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7882         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7883         final Acceleration bx2 = new Acceleration(0.0,
7884                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7885         calibrator.getInitialBiasXAsAcceleration(bx2);
7886         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7887         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7888         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7889         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7890         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7891         final Acceleration by2 = new Acceleration(0.0,
7892                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7893         calibrator.getInitialBiasYAsAcceleration(by2);
7894         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7895         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7896         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7897         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7898         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7899         final Acceleration bz2 = new Acceleration(0.0,
7900                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7901         calibrator.getInitialBiasZAsAcceleration(bz2);
7902         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7903         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7904         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7905         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7906         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7907         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7908         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7909         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7910         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7911         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7912         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7913         final double[] bias1 = calibrator.getInitialBias();
7914         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7915         final double[] bias2 = new double[3];
7916         calibrator.getInitialBias(bias2);
7917         assertArrayEquals(bias1, bias2, 0.0);
7918         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7919         assertEquals(b1, ba);
7920         final Matrix b2 = new Matrix(3, 1);
7921         calibrator.getInitialBiasAsMatrix(b2);
7922         assertEquals(b1, b2);
7923         final Matrix ma1 = calibrator.getInitialMa();
7924         assertEquals(ma1, new Matrix(3, 3));
7925         final Matrix ma2 = new Matrix(3, 3);
7926         calibrator.getInitialMa(ma2);
7927         assertEquals(ma1, ma2);
7928         assertNull(calibrator.getEcefPosition());
7929         assertNull(calibrator.getNedPosition());
7930         assertFalse(calibrator.getNedPosition(null));
7931         assertSame(calibrator.getMeasurements(), measurements);
7932         assertTrue(calibrator.isCommonAxisUsed());
7933         assertSame(calibrator.getListener(), this);
7934         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
7935         assertFalse(calibrator.isReady());
7936         assertFalse(calibrator.isRunning());
7937         assertNull(calibrator.getEstimatedBiases());
7938         assertFalse(calibrator.getEstimatedBiases(null));
7939         assertNull(calibrator.getEstimatedBiasesAsMatrix());
7940         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7941         assertNull(calibrator.getEstimatedBiasFx());
7942         assertNull(calibrator.getEstimatedBiasFy());
7943         assertNull(calibrator.getEstimatedBiasFz());
7944         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7945         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7946         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7947         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7948         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7949         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7950         assertNull(calibrator.getEstimatedMa());
7951         assertNull(calibrator.getEstimatedSx());
7952         assertNull(calibrator.getEstimatedSy());
7953         assertNull(calibrator.getEstimatedSz());
7954         assertNull(calibrator.getEstimatedMxy());
7955         assertNull(calibrator.getEstimatedMxz());
7956         assertNull(calibrator.getEstimatedMyx());
7957         assertNull(calibrator.getEstimatedMyz());
7958         assertNull(calibrator.getEstimatedMzx());
7959         assertNull(calibrator.getEstimatedMzy());
7960         assertNull(calibrator.getEstimatedCovariance());
7961         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7962         assertNull(calibrator.getGroundTruthGravityNorm());
7963         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7964         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7965 
7966         // Force IllegalArgumentException
7967         calibrator = null;
7968         try {
7969             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7970                     true, new Matrix(1, 1),
7971                     this);
7972             fail("IllegalArgumentException expected but not thrown");
7973         } catch (final IllegalArgumentException ignore) {
7974         }
7975         try {
7976             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7977                     true, new Matrix(1, 3),
7978                     this);
7979             fail("IllegalArgumentException expected but not thrown");
7980         } catch (final IllegalArgumentException ignore) {
7981         }
7982         assertNull(calibrator);
7983     }
7984 
7985     @Test
7986     public void testConstructor71() throws WrongSizeException {
7987         final Matrix ba = generateBa();
7988         final double biasX = ba.getElementAtIndex(0);
7989         final double biasY = ba.getElementAtIndex(1);
7990         final double biasZ = ba.getElementAtIndex(2);
7991 
7992         final Matrix ma = generateMaCommonAxis();
7993         final double sx = ma.getElementAt(0, 0);
7994         final double sy = ma.getElementAt(1, 1);
7995         final double sz = ma.getElementAt(2, 2);
7996         final double mxy = ma.getElementAt(0, 1);
7997         final double mxz = ma.getElementAt(0, 2);
7998         final double myx = ma.getElementAt(1, 0);
7999         final double myz = ma.getElementAt(1, 2);
8000         final double mzx = ma.getElementAt(2, 0);
8001         final double mzy = ma.getElementAt(2, 1);
8002 
8003         KnownPositionAccelerometerCalibrator calibrator =
8004                 new KnownPositionAccelerometerCalibrator(ba, ma);
8005 
8006         // check default values
8007         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8008         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8009         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8010         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8011         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8012         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8013         final Acceleration bx2 = new Acceleration(0.0,
8014                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8015         calibrator.getInitialBiasXAsAcceleration(bx2);
8016         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8017         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8018         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8019         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8020         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8021         final Acceleration by2 = new Acceleration(0.0,
8022                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8023         calibrator.getInitialBiasYAsAcceleration(by2);
8024         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8025         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8026         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8027         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8028         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8029         final Acceleration bz2 = new Acceleration(0.0,
8030                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8031         calibrator.getInitialBiasZAsAcceleration(bz2);
8032         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8033         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8034         assertEquals(calibrator.getInitialSx(), sx, 0.0);
8035         assertEquals(calibrator.getInitialSy(), sy, 0.0);
8036         assertEquals(calibrator.getInitialSz(), sz, 0.0);
8037         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8038         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8039         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8040         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8041         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8042         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8043         final double[] bias1 = calibrator.getInitialBias();
8044         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8045         final double[] bias2 = new double[3];
8046         calibrator.getInitialBias(bias2);
8047         assertArrayEquals(bias1, bias2, 0.0);
8048         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8049         assertEquals(b1, ba);
8050         final Matrix b2 = new Matrix(3, 1);
8051         calibrator.getInitialBiasAsMatrix(b2);
8052         assertEquals(b1, b2);
8053         final Matrix ma1 = new Matrix(3, 3);
8054         ma1.setSubmatrix(0, 0,
8055                 2, 2,
8056                 new double[]{sx, myx, mzx,
8057                         mxy, sy, mzy,
8058                         mxz, myz, sz});
8059         assertEquals(calibrator.getInitialMa(), ma1);
8060         final Matrix ma2 = new Matrix(3, 3);
8061         calibrator.getInitialMa(ma2);
8062         assertEquals(ma1, ma2);
8063         assertNull(calibrator.getEcefPosition());
8064         assertNull(calibrator.getNedPosition());
8065         assertFalse(calibrator.getNedPosition(null));
8066         assertNull(calibrator.getMeasurements());
8067         assertFalse(calibrator.isCommonAxisUsed());
8068         assertNull(calibrator.getListener());
8069         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
8070         assertFalse(calibrator.isReady());
8071         assertFalse(calibrator.isRunning());
8072         assertNull(calibrator.getEstimatedBiases());
8073         assertFalse(calibrator.getEstimatedBiases(null));
8074         assertNull(calibrator.getEstimatedBiasesAsMatrix());
8075         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8076         assertNull(calibrator.getEstimatedBiasFx());
8077         assertNull(calibrator.getEstimatedBiasFy());
8078         assertNull(calibrator.getEstimatedBiasFz());
8079         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8080         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8081         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8082         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8083         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8084         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8085         assertNull(calibrator.getEstimatedMa());
8086         assertNull(calibrator.getEstimatedSx());
8087         assertNull(calibrator.getEstimatedSy());
8088         assertNull(calibrator.getEstimatedSz());
8089         assertNull(calibrator.getEstimatedMxy());
8090         assertNull(calibrator.getEstimatedMxz());
8091         assertNull(calibrator.getEstimatedMyx());
8092         assertNull(calibrator.getEstimatedMyz());
8093         assertNull(calibrator.getEstimatedMzx());
8094         assertNull(calibrator.getEstimatedMzy());
8095         assertNull(calibrator.getEstimatedCovariance());
8096         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8097         assertNull(calibrator.getGroundTruthGravityNorm());
8098         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8099         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8100 
8101         // Force IllegalArgumentException
8102         calibrator = null;
8103         try {
8104             calibrator = new KnownPositionAccelerometerCalibrator(
8105                     new Matrix(1, 1), ma);
8106             fail("IllegalArgumentException expected but not thrown");
8107         } catch (final IllegalArgumentException ignore) {
8108         }
8109         try {
8110             calibrator = new KnownPositionAccelerometerCalibrator(
8111                     new Matrix(1, 3), ma);
8112             fail("IllegalArgumentException expected but not thrown");
8113         } catch (final IllegalArgumentException ignore) {
8114         }
8115         try {
8116             calibrator = new KnownPositionAccelerometerCalibrator(ba,
8117                     new Matrix(1, 3));
8118             fail("IllegalArgumentException expected but not thrown");
8119         } catch (final IllegalArgumentException ignore) {
8120         }
8121         try {
8122             calibrator = new KnownPositionAccelerometerCalibrator(ba,
8123                     new Matrix(3, 1));
8124             fail("IllegalArgumentException expected but not thrown");
8125         } catch (final IllegalArgumentException ignore) {
8126         }
8127         assertNull(calibrator);
8128     }
8129 
8130     @Test
8131     public void testConstructor72() throws WrongSizeException {
8132         final Matrix ba = generateBa();
8133         final double biasX = ba.getElementAtIndex(0);
8134         final double biasY = ba.getElementAtIndex(1);
8135         final double biasZ = ba.getElementAtIndex(2);
8136 
8137         final Matrix ma = generateMaCommonAxis();
8138         final double sx = ma.getElementAt(0, 0);
8139         final double sy = ma.getElementAt(1, 1);
8140         final double sz = ma.getElementAt(2, 2);
8141         final double mxy = ma.getElementAt(0, 1);
8142         final double mxz = ma.getElementAt(0, 2);
8143         final double myx = ma.getElementAt(1, 0);
8144         final double myz = ma.getElementAt(1, 2);
8145         final double mzx = ma.getElementAt(2, 0);
8146         final double mzy = ma.getElementAt(2, 1);
8147 
8148         KnownPositionAccelerometerCalibrator calibrator =
8149                 new KnownPositionAccelerometerCalibrator(ba, ma, this);
8150 
8151         // check default values
8152         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8153         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8154         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8155         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8156         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8157         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8158         final Acceleration bx2 = new Acceleration(0.0,
8159                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8160         calibrator.getInitialBiasXAsAcceleration(bx2);
8161         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8162         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8163         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8164         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8165         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8166         final Acceleration by2 = new Acceleration(0.0,
8167                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8168         calibrator.getInitialBiasYAsAcceleration(by2);
8169         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8170         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8171         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8172         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8173         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8174         final Acceleration bz2 = new Acceleration(0.0,
8175                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8176         calibrator.getInitialBiasZAsAcceleration(bz2);
8177         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8178         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8179         assertEquals(calibrator.getInitialSx(), sx, 0.0);
8180         assertEquals(calibrator.getInitialSy(), sy, 0.0);
8181         assertEquals(calibrator.getInitialSz(), sz, 0.0);
8182         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8183         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8184         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8185         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8186         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8187         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8188         final double[] bias1 = calibrator.getInitialBias();
8189         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8190         final double[] bias2 = new double[3];
8191         calibrator.getInitialBias(bias2);
8192         assertArrayEquals(bias1, bias2, 0.0);
8193         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8194         assertEquals(b1, ba);
8195         final Matrix b2 = new Matrix(3, 1);
8196         calibrator.getInitialBiasAsMatrix(b2);
8197         assertEquals(b1, b2);
8198         final Matrix ma1 = new Matrix(3, 3);
8199         ma1.setSubmatrix(0, 0,
8200                 2, 2,
8201                 new double[]{sx, myx, mzx,
8202                         mxy, sy, mzy,
8203                         mxz, myz, sz});
8204         assertEquals(calibrator.getInitialMa(), ma1);
8205         final Matrix ma2 = new Matrix(3, 3);
8206         calibrator.getInitialMa(ma2);
8207         assertEquals(ma1, ma2);
8208         assertNull(calibrator.getEcefPosition());
8209         assertNull(calibrator.getNedPosition());
8210         assertFalse(calibrator.getNedPosition(null));
8211         assertNull(calibrator.getMeasurements());
8212         assertFalse(calibrator.isCommonAxisUsed());
8213         assertSame(calibrator.getListener(), this);
8214         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
8215         assertFalse(calibrator.isReady());
8216         assertFalse(calibrator.isRunning());
8217         assertNull(calibrator.getEstimatedBiases());
8218         assertFalse(calibrator.getEstimatedBiases(null));
8219         assertNull(calibrator.getEstimatedBiasesAsMatrix());
8220         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8221         assertNull(calibrator.getEstimatedBiasFx());
8222         assertNull(calibrator.getEstimatedBiasFy());
8223         assertNull(calibrator.getEstimatedBiasFz());
8224         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8225         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8226         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8227         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8228         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8229         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8230         assertNull(calibrator.getEstimatedMa());
8231         assertNull(calibrator.getEstimatedSx());
8232         assertNull(calibrator.getEstimatedSy());
8233         assertNull(calibrator.getEstimatedSz());
8234         assertNull(calibrator.getEstimatedMxy());
8235         assertNull(calibrator.getEstimatedMxz());
8236         assertNull(calibrator.getEstimatedMyx());
8237         assertNull(calibrator.getEstimatedMyz());
8238         assertNull(calibrator.getEstimatedMzx());
8239         assertNull(calibrator.getEstimatedMzy());
8240         assertNull(calibrator.getEstimatedCovariance());
8241         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8242         assertNull(calibrator.getGroundTruthGravityNorm());
8243         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8244         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8245 
8246         // Force IllegalArgumentException
8247         calibrator = null;
8248         try {
8249             calibrator = new KnownPositionAccelerometerCalibrator(
8250                     new Matrix(1, 1), ma, this);
8251             fail("IllegalArgumentException expected but not thrown");
8252         } catch (final IllegalArgumentException ignore) {
8253         }
8254         try {
8255             calibrator = new KnownPositionAccelerometerCalibrator(
8256                     new Matrix(1, 3), ma, this);
8257             fail("IllegalArgumentException expected but not thrown");
8258         } catch (final IllegalArgumentException ignore) {
8259         }
8260         try {
8261             calibrator = new KnownPositionAccelerometerCalibrator(ba,
8262                     new Matrix(1, 3), this);
8263             fail("IllegalArgumentException expected but not thrown");
8264         } catch (final IllegalArgumentException ignore) {
8265         }
8266         try {
8267             calibrator = new KnownPositionAccelerometerCalibrator(ba,
8268                     new Matrix(3, 1), this);
8269             fail("IllegalArgumentException expected but not thrown");
8270         } catch (final IllegalArgumentException ignore) {
8271         }
8272         assertNull(calibrator);
8273     }
8274 
8275     @Test
8276     public void testConstructor73() throws WrongSizeException {
8277         final Collection<StandardDeviationBodyKinematics> measurements =
8278                 Collections.emptyList();
8279 
8280         final Matrix ba = generateBa();
8281         final double biasX = ba.getElementAtIndex(0);
8282         final double biasY = ba.getElementAtIndex(1);
8283         final double biasZ = ba.getElementAtIndex(2);
8284 
8285         final Matrix ma = generateMaCommonAxis();
8286         final double sx = ma.getElementAt(0, 0);
8287         final double sy = ma.getElementAt(1, 1);
8288         final double sz = ma.getElementAt(2, 2);
8289         final double mxy = ma.getElementAt(0, 1);
8290         final double mxz = ma.getElementAt(0, 2);
8291         final double myx = ma.getElementAt(1, 0);
8292         final double myz = ma.getElementAt(1, 2);
8293         final double mzx = ma.getElementAt(2, 0);
8294         final double mzy = ma.getElementAt(2, 1);
8295 
8296         KnownPositionAccelerometerCalibrator calibrator =
8297                 new KnownPositionAccelerometerCalibrator(measurements, ba, ma);
8298 
8299         // check default values
8300         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8301         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8302         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8303         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8304         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8305         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8306         final Acceleration bx2 = new Acceleration(0.0,
8307                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8308         calibrator.getInitialBiasXAsAcceleration(bx2);
8309         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8310         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8311         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8312         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8313         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8314         final Acceleration by2 = new Acceleration(0.0,
8315                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8316         calibrator.getInitialBiasYAsAcceleration(by2);
8317         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8318         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8319         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8320         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8321         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8322         final Acceleration bz2 = new Acceleration(0.0,
8323                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8324         calibrator.getInitialBiasZAsAcceleration(bz2);
8325         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8326         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8327         assertEquals(calibrator.getInitialSx(), sx, 0.0);
8328         assertEquals(calibrator.getInitialSy(), sy, 0.0);
8329         assertEquals(calibrator.getInitialSz(), sz, 0.0);
8330         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8331         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8332         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8333         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8334         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8335         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8336         final double[] bias1 = calibrator.getInitialBias();
8337         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8338         final double[] bias2 = new double[3];
8339         calibrator.getInitialBias(bias2);
8340         assertArrayEquals(bias1, bias2, 0.0);
8341         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8342         assertEquals(b1, ba);
8343         final Matrix b2 = new Matrix(3, 1);
8344         calibrator.getInitialBiasAsMatrix(b2);
8345         assertEquals(b1, b2);
8346         final Matrix ma1 = new Matrix(3, 3);
8347         ma1.setSubmatrix(0, 0,
8348                 2, 2,
8349                 new double[]{sx, myx, mzx,
8350                         mxy, sy, mzy,
8351                         mxz, myz, sz});
8352         assertEquals(calibrator.getInitialMa(), ma1);
8353         final Matrix ma2 = new Matrix(3, 3);
8354         calibrator.getInitialMa(ma2);
8355         assertEquals(ma1, ma2);
8356         assertNull(calibrator.getEcefPosition());
8357         assertNull(calibrator.getNedPosition());
8358         assertFalse(calibrator.getNedPosition(null));
8359         assertSame(calibrator.getMeasurements(), measurements);
8360         assertFalse(calibrator.isCommonAxisUsed());
8361         assertNull(calibrator.getListener());
8362         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
8363         assertFalse(calibrator.isReady());
8364         assertFalse(calibrator.isRunning());
8365         assertNull(calibrator.getEstimatedBiases());
8366         assertFalse(calibrator.getEstimatedBiases(null));
8367         assertNull(calibrator.getEstimatedBiasesAsMatrix());
8368         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8369         assertNull(calibrator.getEstimatedBiasFx());
8370         assertNull(calibrator.getEstimatedBiasFy());
8371         assertNull(calibrator.getEstimatedBiasFz());
8372         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8373         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8374         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8375         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8376         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8377         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8378         assertNull(calibrator.getEstimatedMa());
8379         assertNull(calibrator.getEstimatedSx());
8380         assertNull(calibrator.getEstimatedSy());
8381         assertNull(calibrator.getEstimatedSz());
8382         assertNull(calibrator.getEstimatedMxy());
8383         assertNull(calibrator.getEstimatedMxz());
8384         assertNull(calibrator.getEstimatedMyx());
8385         assertNull(calibrator.getEstimatedMyz());
8386         assertNull(calibrator.getEstimatedMzx());
8387         assertNull(calibrator.getEstimatedMzy());
8388         assertNull(calibrator.getEstimatedCovariance());
8389         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8390         assertNull(calibrator.getGroundTruthGravityNorm());
8391         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8392         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8393 
8394         // Force IllegalArgumentException
8395         calibrator = null;
8396         try {
8397             calibrator = new KnownPositionAccelerometerCalibrator(
8398                     measurements, new Matrix(1, 1), ma);
8399             fail("IllegalArgumentException expected but not thrown");
8400         } catch (final IllegalArgumentException ignore) {
8401         }
8402         try {
8403             calibrator = new KnownPositionAccelerometerCalibrator(
8404                     measurements, new Matrix(1, 3), ma);
8405             fail("IllegalArgumentException expected but not thrown");
8406         } catch (final IllegalArgumentException ignore) {
8407         }
8408         try {
8409             calibrator = new KnownPositionAccelerometerCalibrator(
8410                     measurements, ba, new Matrix(1, 3));
8411             fail("IllegalArgumentException expected but not thrown");
8412         } catch (final IllegalArgumentException ignore) {
8413         }
8414         try {
8415             calibrator = new KnownPositionAccelerometerCalibrator(
8416                     measurements, ba, new Matrix(3, 1));
8417             fail("IllegalArgumentException expected but not thrown");
8418         } catch (final IllegalArgumentException ignore) {
8419         }
8420         assertNull(calibrator);
8421     }
8422 
8423     @Test
8424     public void testConstructor74() throws WrongSizeException {
8425         final Collection<StandardDeviationBodyKinematics> measurements =
8426                 Collections.emptyList();
8427 
8428         final Matrix ba = generateBa();
8429         final double biasX = ba.getElementAtIndex(0);
8430         final double biasY = ba.getElementAtIndex(1);
8431         final double biasZ = ba.getElementAtIndex(2);
8432 
8433         final Matrix ma = generateMaCommonAxis();
8434         final double sx = ma.getElementAt(0, 0);
8435         final double sy = ma.getElementAt(1, 1);
8436         final double sz = ma.getElementAt(2, 2);
8437         final double mxy = ma.getElementAt(0, 1);
8438         final double mxz = ma.getElementAt(0, 2);
8439         final double myx = ma.getElementAt(1, 0);
8440         final double myz = ma.getElementAt(1, 2);
8441         final double mzx = ma.getElementAt(2, 0);
8442         final double mzy = ma.getElementAt(2, 1);
8443 
8444         KnownPositionAccelerometerCalibrator calibrator =
8445                 new KnownPositionAccelerometerCalibrator(measurements, ba, ma,
8446                         this);
8447 
8448         // check default values
8449         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8450         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8451         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8452         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8453         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8454         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8455         final Acceleration bx2 = new Acceleration(0.0,
8456                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8457         calibrator.getInitialBiasXAsAcceleration(bx2);
8458         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8459         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8460         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8461         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8462         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8463         final Acceleration by2 = new Acceleration(0.0,
8464                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8465         calibrator.getInitialBiasYAsAcceleration(by2);
8466         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8467         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8468         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8469         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8470         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8471         final Acceleration bz2 = new Acceleration(0.0,
8472                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8473         calibrator.getInitialBiasZAsAcceleration(bz2);
8474         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8475         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8476         assertEquals(calibrator.getInitialSx(), sx, 0.0);
8477         assertEquals(calibrator.getInitialSy(), sy, 0.0);
8478         assertEquals(calibrator.getInitialSz(), sz, 0.0);
8479         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8480         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8481         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8482         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8483         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8484         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8485         final double[] bias1 = calibrator.getInitialBias();
8486         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8487         final double[] bias2 = new double[3];
8488         calibrator.getInitialBias(bias2);
8489         assertArrayEquals(bias1, bias2, 0.0);
8490         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8491         assertEquals(b1, ba);
8492         final Matrix b2 = new Matrix(3, 1);
8493         calibrator.getInitialBiasAsMatrix(b2);
8494         assertEquals(b1, b2);
8495         final Matrix ma1 = new Matrix(3, 3);
8496         ma1.setSubmatrix(0, 0,
8497                 2, 2,
8498                 new double[]{sx, myx, mzx,
8499                         mxy, sy, mzy,
8500                         mxz, myz, sz});
8501         assertEquals(calibrator.getInitialMa(), ma1);
8502         final Matrix ma2 = new Matrix(3, 3);
8503         calibrator.getInitialMa(ma2);
8504         assertEquals(ma1, ma2);
8505         assertNull(calibrator.getEcefPosition());
8506         assertNull(calibrator.getNedPosition());
8507         assertFalse(calibrator.getNedPosition(null));
8508         assertSame(calibrator.getMeasurements(), measurements);
8509         assertFalse(calibrator.isCommonAxisUsed());
8510         assertSame(calibrator.getListener(), this);
8511         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
8512         assertFalse(calibrator.isReady());
8513         assertFalse(calibrator.isRunning());
8514         assertNull(calibrator.getEstimatedBiases());
8515         assertFalse(calibrator.getEstimatedBiases(null));
8516         assertNull(calibrator.getEstimatedBiasesAsMatrix());
8517         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8518         assertNull(calibrator.getEstimatedBiasFx());
8519         assertNull(calibrator.getEstimatedBiasFy());
8520         assertNull(calibrator.getEstimatedBiasFz());
8521         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8522         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8523         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8524         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8525         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8526         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8527         assertNull(calibrator.getEstimatedMa());
8528         assertNull(calibrator.getEstimatedSx());
8529         assertNull(calibrator.getEstimatedSy());
8530         assertNull(calibrator.getEstimatedSz());
8531         assertNull(calibrator.getEstimatedMxy());
8532         assertNull(calibrator.getEstimatedMxz());
8533         assertNull(calibrator.getEstimatedMyx());
8534         assertNull(calibrator.getEstimatedMyz());
8535         assertNull(calibrator.getEstimatedMzx());
8536         assertNull(calibrator.getEstimatedMzy());
8537         assertNull(calibrator.getEstimatedCovariance());
8538         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8539         assertNull(calibrator.getGroundTruthGravityNorm());
8540         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8541         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8542 
8543         // Force IllegalArgumentException
8544         calibrator = null;
8545         try {
8546             calibrator = new KnownPositionAccelerometerCalibrator(
8547                     measurements, new Matrix(1, 1), ma, this);
8548             fail("IllegalArgumentException expected but not thrown");
8549         } catch (final IllegalArgumentException ignore) {
8550         }
8551         try {
8552             calibrator = new KnownPositionAccelerometerCalibrator(
8553                     measurements, new Matrix(1, 3), ma, this);
8554             fail("IllegalArgumentException expected but not thrown");
8555         } catch (final IllegalArgumentException ignore) {
8556         }
8557         try {
8558             calibrator = new KnownPositionAccelerometerCalibrator(
8559                     measurements, ba, new Matrix(1, 3), this);
8560             fail("IllegalArgumentException expected but not thrown");
8561         } catch (final IllegalArgumentException ignore) {
8562         }
8563         try {
8564             calibrator = new KnownPositionAccelerometerCalibrator(
8565                     measurements, ba, new Matrix(3, 1), this);
8566             fail("IllegalArgumentException expected but not thrown");
8567         } catch (final IllegalArgumentException ignore) {
8568         }
8569         assertNull(calibrator);
8570     }
8571 
8572     @Test
8573     public void testConstructor75() throws WrongSizeException {
8574         final Matrix ba = generateBa();
8575         final double biasX = ba.getElementAtIndex(0);
8576         final double biasY = ba.getElementAtIndex(1);
8577         final double biasZ = ba.getElementAtIndex(2);
8578 
8579         final Matrix ma = generateMaCommonAxis();
8580         final double sx = ma.getElementAt(0, 0);
8581         final double sy = ma.getElementAt(1, 1);
8582         final double sz = ma.getElementAt(2, 2);
8583         final double mxy = ma.getElementAt(0, 1);
8584         final double mxz = ma.getElementAt(0, 2);
8585         final double myx = ma.getElementAt(1, 0);
8586         final double myz = ma.getElementAt(1, 2);
8587         final double mzx = ma.getElementAt(2, 0);
8588         final double mzy = ma.getElementAt(2, 1);
8589 
8590         KnownPositionAccelerometerCalibrator calibrator =
8591                 new KnownPositionAccelerometerCalibrator(true, ba, ma);
8592 
8593         // check default values
8594         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8595         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8596         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8597         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8598         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8599         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8600         final Acceleration bx2 = new Acceleration(0.0,
8601                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8602         calibrator.getInitialBiasXAsAcceleration(bx2);
8603         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8604         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8605         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8606         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8607         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8608         final Acceleration by2 = new Acceleration(0.0,
8609                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8610         calibrator.getInitialBiasYAsAcceleration(by2);
8611         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8612         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8613         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8614         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8615         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8616         final Acceleration bz2 = new Acceleration(0.0,
8617                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8618         calibrator.getInitialBiasZAsAcceleration(bz2);
8619         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8620         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8621         assertEquals(calibrator.getInitialSx(), sx, 0.0);
8622         assertEquals(calibrator.getInitialSy(), sy, 0.0);
8623         assertEquals(calibrator.getInitialSz(), sz, 0.0);
8624         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8625         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8626         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8627         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8628         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8629         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8630         final double[] bias1 = calibrator.getInitialBias();
8631         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8632         final double[] bias2 = new double[3];
8633         calibrator.getInitialBias(bias2);
8634         assertArrayEquals(bias1, bias2, 0.0);
8635         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8636         assertEquals(b1, ba);
8637         final Matrix b2 = new Matrix(3, 1);
8638         calibrator.getInitialBiasAsMatrix(b2);
8639         assertEquals(b1, b2);
8640         final Matrix ma1 = new Matrix(3, 3);
8641         ma1.setSubmatrix(0, 0,
8642                 2, 2,
8643                 new double[]{sx, myx, mzx,
8644                         mxy, sy, mzy,
8645                         mxz, myz, sz});
8646         assertEquals(calibrator.getInitialMa(), ma1);
8647         final Matrix ma2 = new Matrix(3, 3);
8648         calibrator.getInitialMa(ma2);
8649         assertEquals(ma1, ma2);
8650         assertNull(calibrator.getEcefPosition());
8651         assertNull(calibrator.getNedPosition());
8652         assertFalse(calibrator.getNedPosition(null));
8653         assertNull(calibrator.getMeasurements());
8654         assertTrue(calibrator.isCommonAxisUsed());
8655         assertNull(calibrator.getListener());
8656         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
8657         assertFalse(calibrator.isReady());
8658         assertFalse(calibrator.isRunning());
8659         assertNull(calibrator.getEstimatedBiases());
8660         assertFalse(calibrator.getEstimatedBiases(null));
8661         assertNull(calibrator.getEstimatedBiasesAsMatrix());
8662         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8663         assertNull(calibrator.getEstimatedBiasFx());
8664         assertNull(calibrator.getEstimatedBiasFy());
8665         assertNull(calibrator.getEstimatedBiasFz());
8666         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8667         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8668         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8669         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8670         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8671         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8672         assertNull(calibrator.getEstimatedMa());
8673         assertNull(calibrator.getEstimatedSx());
8674         assertNull(calibrator.getEstimatedSy());
8675         assertNull(calibrator.getEstimatedSz());
8676         assertNull(calibrator.getEstimatedMxy());
8677         assertNull(calibrator.getEstimatedMxz());
8678         assertNull(calibrator.getEstimatedMyx());
8679         assertNull(calibrator.getEstimatedMyz());
8680         assertNull(calibrator.getEstimatedMzx());
8681         assertNull(calibrator.getEstimatedMzy());
8682         assertNull(calibrator.getEstimatedCovariance());
8683         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8684         assertNull(calibrator.getGroundTruthGravityNorm());
8685         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8686         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8687 
8688         // Force IllegalArgumentException
8689         calibrator = null;
8690         try {
8691             calibrator = new KnownPositionAccelerometerCalibrator(
8692                     true, new Matrix(1, 1), ma);
8693             fail("IllegalArgumentException expected but not thrown");
8694         } catch (final IllegalArgumentException ignore) {
8695         }
8696         try {
8697             calibrator = new KnownPositionAccelerometerCalibrator(
8698                     true, new Matrix(1, 3), ma);
8699             fail("IllegalArgumentException expected but not thrown");
8700         } catch (final IllegalArgumentException ignore) {
8701         }
8702         try {
8703             calibrator = new KnownPositionAccelerometerCalibrator(
8704                     true, ba, new Matrix(1, 3));
8705             fail("IllegalArgumentException expected but not thrown");
8706         } catch (final IllegalArgumentException ignore) {
8707         }
8708         try {
8709             calibrator = new KnownPositionAccelerometerCalibrator(
8710                     true, ba, new Matrix(3, 1));
8711             fail("IllegalArgumentException expected but not thrown");
8712         } catch (final IllegalArgumentException ignore) {
8713         }
8714         assertNull(calibrator);
8715     }
8716 
8717     @Test
8718     public void testConstructor76() throws WrongSizeException {
8719         final Matrix ba = generateBa();
8720         final double biasX = ba.getElementAtIndex(0);
8721         final double biasY = ba.getElementAtIndex(1);
8722         final double biasZ = ba.getElementAtIndex(2);
8723 
8724         final Matrix ma = generateMaCommonAxis();
8725         final double sx = ma.getElementAt(0, 0);
8726         final double sy = ma.getElementAt(1, 1);
8727         final double sz = ma.getElementAt(2, 2);
8728         final double mxy = ma.getElementAt(0, 1);
8729         final double mxz = ma.getElementAt(0, 2);
8730         final double myx = ma.getElementAt(1, 0);
8731         final double myz = ma.getElementAt(1, 2);
8732         final double mzx = ma.getElementAt(2, 0);
8733         final double mzy = ma.getElementAt(2, 1);
8734 
8735         KnownPositionAccelerometerCalibrator calibrator =
8736                 new KnownPositionAccelerometerCalibrator(true, ba, ma,
8737                         this);
8738 
8739         // check default values
8740         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8741         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8742         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8743         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8744         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8745         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8746         final Acceleration bx2 = new Acceleration(0.0,
8747                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8748         calibrator.getInitialBiasXAsAcceleration(bx2);
8749         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8750         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8751         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8752         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8753         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8754         final Acceleration by2 = new Acceleration(0.0,
8755                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8756         calibrator.getInitialBiasYAsAcceleration(by2);
8757         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8758         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8759         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8760         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8761         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8762         final Acceleration bz2 = new Acceleration(0.0,
8763                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8764         calibrator.getInitialBiasZAsAcceleration(bz2);
8765         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8766         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8767         assertEquals(calibrator.getInitialSx(), sx, 0.0);
8768         assertEquals(calibrator.getInitialSy(), sy, 0.0);
8769         assertEquals(calibrator.getInitialSz(), sz, 0.0);
8770         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8771         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8772         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8773         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8774         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8775         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8776         final double[] bias1 = calibrator.getInitialBias();
8777         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8778         final double[] bias2 = new double[3];
8779         calibrator.getInitialBias(bias2);
8780         assertArrayEquals(bias1, bias2, 0.0);
8781         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8782         assertEquals(b1, ba);
8783         final Matrix b2 = new Matrix(3, 1);
8784         calibrator.getInitialBiasAsMatrix(b2);
8785         assertEquals(b1, b2);
8786         final Matrix ma1 = new Matrix(3, 3);
8787         ma1.setSubmatrix(0, 0,
8788                 2, 2,
8789                 new double[]{sx, myx, mzx,
8790                         mxy, sy, mzy,
8791                         mxz, myz, sz});
8792         assertEquals(calibrator.getInitialMa(), ma1);
8793         final Matrix ma2 = new Matrix(3, 3);
8794         calibrator.getInitialMa(ma2);
8795         assertEquals(ma1, ma2);
8796         assertNull(calibrator.getEcefPosition());
8797         assertNull(calibrator.getNedPosition());
8798         assertFalse(calibrator.getNedPosition(null));
8799         assertNull(calibrator.getMeasurements());
8800         assertTrue(calibrator.isCommonAxisUsed());
8801         assertSame(calibrator.getListener(), this);
8802         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
8803         assertFalse(calibrator.isReady());
8804         assertFalse(calibrator.isRunning());
8805         assertNull(calibrator.getEstimatedBiases());
8806         assertFalse(calibrator.getEstimatedBiases(null));
8807         assertNull(calibrator.getEstimatedBiasesAsMatrix());
8808         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8809         assertNull(calibrator.getEstimatedBiasFx());
8810         assertNull(calibrator.getEstimatedBiasFy());
8811         assertNull(calibrator.getEstimatedBiasFz());
8812         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8813         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8814         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8815         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8816         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8817         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8818         assertNull(calibrator.getEstimatedMa());
8819         assertNull(calibrator.getEstimatedSx());
8820         assertNull(calibrator.getEstimatedSy());
8821         assertNull(calibrator.getEstimatedSz());
8822         assertNull(calibrator.getEstimatedMxy());
8823         assertNull(calibrator.getEstimatedMxz());
8824         assertNull(calibrator.getEstimatedMyx());
8825         assertNull(calibrator.getEstimatedMyz());
8826         assertNull(calibrator.getEstimatedMzx());
8827         assertNull(calibrator.getEstimatedMzy());
8828         assertNull(calibrator.getEstimatedCovariance());
8829         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8830         assertNull(calibrator.getGroundTruthGravityNorm());
8831         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8832         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8833 
8834         // Force IllegalArgumentException
8835         calibrator = null;
8836         try {
8837             calibrator = new KnownPositionAccelerometerCalibrator(
8838                     true, new Matrix(1, 1), ma,
8839                     this);
8840             fail("IllegalArgumentException expected but not thrown");
8841         } catch (final IllegalArgumentException ignore) {
8842         }
8843         try {
8844             calibrator = new KnownPositionAccelerometerCalibrator(
8845                     true, new Matrix(1, 3), ma,
8846                     this);
8847             fail("IllegalArgumentException expected but not thrown");
8848         } catch (final IllegalArgumentException ignore) {
8849         }
8850         try {
8851             calibrator = new KnownPositionAccelerometerCalibrator(
8852                     true, ba, new Matrix(1, 3),
8853                     this);
8854             fail("IllegalArgumentException expected but not thrown");
8855         } catch (final IllegalArgumentException ignore) {
8856         }
8857         try {
8858             calibrator = new KnownPositionAccelerometerCalibrator(
8859                     true, ba, new Matrix(3, 1),
8860                     this);
8861             fail("IllegalArgumentException expected but not thrown");
8862         } catch (final IllegalArgumentException ignore) {
8863         }
8864         assertNull(calibrator);
8865     }
8866 
8867     @Test
8868     public void testConstructor77() throws WrongSizeException {
8869         final Collection<StandardDeviationBodyKinematics> measurements =
8870                 Collections.emptyList();
8871 
8872         final Matrix ba = generateBa();
8873         final double biasX = ba.getElementAtIndex(0);
8874         final double biasY = ba.getElementAtIndex(1);
8875         final double biasZ = ba.getElementAtIndex(2);
8876 
8877         final Matrix ma = generateMaCommonAxis();
8878         final double sx = ma.getElementAt(0, 0);
8879         final double sy = ma.getElementAt(1, 1);
8880         final double sz = ma.getElementAt(2, 2);
8881         final double mxy = ma.getElementAt(0, 1);
8882         final double mxz = ma.getElementAt(0, 2);
8883         final double myx = ma.getElementAt(1, 0);
8884         final double myz = ma.getElementAt(1, 2);
8885         final double mzx = ma.getElementAt(2, 0);
8886         final double mzy = ma.getElementAt(2, 1);
8887 
8888         KnownPositionAccelerometerCalibrator calibrator =
8889                 new KnownPositionAccelerometerCalibrator(measurements,
8890                         true, ba, ma);
8891 
8892         // check default values
8893         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8894         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8895         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8896         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8897         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8898         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8899         final Acceleration bx2 = new Acceleration(0.0,
8900                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8901         calibrator.getInitialBiasXAsAcceleration(bx2);
8902         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8903         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8904         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8905         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8906         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8907         final Acceleration by2 = new Acceleration(0.0,
8908                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8909         calibrator.getInitialBiasYAsAcceleration(by2);
8910         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8911         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8912         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8913         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8914         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8915         final Acceleration bz2 = new Acceleration(0.0,
8916                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8917         calibrator.getInitialBiasZAsAcceleration(bz2);
8918         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8919         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8920         assertEquals(calibrator.getInitialSx(), sx, 0.0);
8921         assertEquals(calibrator.getInitialSy(), sy, 0.0);
8922         assertEquals(calibrator.getInitialSz(), sz, 0.0);
8923         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8924         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8925         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8926         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8927         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8928         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8929         final double[] bias1 = calibrator.getInitialBias();
8930         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8931         final double[] bias2 = new double[3];
8932         calibrator.getInitialBias(bias2);
8933         assertArrayEquals(bias1, bias2, 0.0);
8934         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8935         assertEquals(b1, ba);
8936         final Matrix b2 = new Matrix(3, 1);
8937         calibrator.getInitialBiasAsMatrix(b2);
8938         assertEquals(b1, b2);
8939         final Matrix ma1 = new Matrix(3, 3);
8940         ma1.setSubmatrix(0, 0,
8941                 2, 2,
8942                 new double[]{sx, myx, mzx,
8943                         mxy, sy, mzy,
8944                         mxz, myz, sz});
8945         assertEquals(calibrator.getInitialMa(), ma1);
8946         final Matrix ma2 = new Matrix(3, 3);
8947         calibrator.getInitialMa(ma2);
8948         assertEquals(ma1, ma2);
8949         assertNull(calibrator.getEcefPosition());
8950         assertNull(calibrator.getNedPosition());
8951         assertFalse(calibrator.getNedPosition(null));
8952         assertSame(calibrator.getMeasurements(), measurements);
8953         assertTrue(calibrator.isCommonAxisUsed());
8954         assertNull(calibrator.getListener());
8955         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
8956         assertFalse(calibrator.isReady());
8957         assertFalse(calibrator.isRunning());
8958         assertNull(calibrator.getEstimatedBiases());
8959         assertFalse(calibrator.getEstimatedBiases(null));
8960         assertNull(calibrator.getEstimatedBiasesAsMatrix());
8961         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8962         assertNull(calibrator.getEstimatedBiasFx());
8963         assertNull(calibrator.getEstimatedBiasFy());
8964         assertNull(calibrator.getEstimatedBiasFz());
8965         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8966         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8967         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8968         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8969         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8970         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8971         assertNull(calibrator.getEstimatedMa());
8972         assertNull(calibrator.getEstimatedSx());
8973         assertNull(calibrator.getEstimatedSy());
8974         assertNull(calibrator.getEstimatedSz());
8975         assertNull(calibrator.getEstimatedMxy());
8976         assertNull(calibrator.getEstimatedMxz());
8977         assertNull(calibrator.getEstimatedMyx());
8978         assertNull(calibrator.getEstimatedMyz());
8979         assertNull(calibrator.getEstimatedMzx());
8980         assertNull(calibrator.getEstimatedMzy());
8981         assertNull(calibrator.getEstimatedCovariance());
8982         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8983         assertNull(calibrator.getGroundTruthGravityNorm());
8984         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8985         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8986 
8987         // Force IllegalArgumentException
8988         calibrator = null;
8989         try {
8990             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
8991                     true, new Matrix(1, 1), ma);
8992             fail("IllegalArgumentException expected but not thrown");
8993         } catch (final IllegalArgumentException ignore) {
8994         }
8995         try {
8996             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
8997                     true, new Matrix(1, 3), ma);
8998             fail("IllegalArgumentException expected but not thrown");
8999         } catch (final IllegalArgumentException ignore) {
9000         }
9001         try {
9002             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
9003                     true, ba, new Matrix(1, 3));
9004             fail("IllegalArgumentException expected but not thrown");
9005         } catch (final IllegalArgumentException ignore) {
9006         }
9007         try {
9008             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
9009                     true, ba, new Matrix(3, 1));
9010             fail("IllegalArgumentException expected but not thrown");
9011         } catch (final IllegalArgumentException ignore) {
9012         }
9013         assertNull(calibrator);
9014     }
9015 
9016     @Test
9017     public void testConstructor78() throws WrongSizeException {
9018         final Collection<StandardDeviationBodyKinematics> measurements =
9019                 Collections.emptyList();
9020 
9021         final Matrix ba = generateBa();
9022         final double biasX = ba.getElementAtIndex(0);
9023         final double biasY = ba.getElementAtIndex(1);
9024         final double biasZ = ba.getElementAtIndex(2);
9025 
9026         final Matrix ma = generateMaCommonAxis();
9027         final double sx = ma.getElementAt(0, 0);
9028         final double sy = ma.getElementAt(1, 1);
9029         final double sz = ma.getElementAt(2, 2);
9030         final double mxy = ma.getElementAt(0, 1);
9031         final double mxz = ma.getElementAt(0, 2);
9032         final double myx = ma.getElementAt(1, 0);
9033         final double myz = ma.getElementAt(1, 2);
9034         final double mzx = ma.getElementAt(2, 0);
9035         final double mzy = ma.getElementAt(2, 1);
9036 
9037         KnownPositionAccelerometerCalibrator calibrator =
9038                 new KnownPositionAccelerometerCalibrator(measurements,
9039                         true, ba, ma, this);
9040 
9041         // check default values
9042         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
9043         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
9044         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
9045         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9046         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
9047         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9048         final Acceleration bx2 = new Acceleration(0.0,
9049                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9050         calibrator.getInitialBiasXAsAcceleration(bx2);
9051         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
9052         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9053         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9054         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
9055         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9056         final Acceleration by2 = new Acceleration(0.0,
9057                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9058         calibrator.getInitialBiasYAsAcceleration(by2);
9059         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
9060         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9061         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9062         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
9063         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9064         final Acceleration bz2 = new Acceleration(0.0,
9065                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9066         calibrator.getInitialBiasZAsAcceleration(bz2);
9067         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
9068         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9069         assertEquals(calibrator.getInitialSx(), sx, 0.0);
9070         assertEquals(calibrator.getInitialSy(), sy, 0.0);
9071         assertEquals(calibrator.getInitialSz(), sz, 0.0);
9072         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
9073         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
9074         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
9075         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
9076         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
9077         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
9078         final double[] bias1 = calibrator.getInitialBias();
9079         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
9080         final double[] bias2 = new double[3];
9081         calibrator.getInitialBias(bias2);
9082         assertArrayEquals(bias1, bias2, 0.0);
9083         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9084         assertEquals(b1, ba);
9085         final Matrix b2 = new Matrix(3, 1);
9086         calibrator.getInitialBiasAsMatrix(b2);
9087         assertEquals(b1, b2);
9088         final Matrix ma1 = new Matrix(3, 3);
9089         ma1.setSubmatrix(0, 0,
9090                 2, 2,
9091                 new double[]{sx, myx, mzx,
9092                         mxy, sy, mzy,
9093                         mxz, myz, sz});
9094         assertEquals(calibrator.getInitialMa(), ma1);
9095         final Matrix ma2 = new Matrix(3, 3);
9096         calibrator.getInitialMa(ma2);
9097         assertEquals(ma1, ma2);
9098         assertNull(calibrator.getEcefPosition());
9099         assertNull(calibrator.getNedPosition());
9100         assertFalse(calibrator.getNedPosition(null));
9101         assertSame(calibrator.getMeasurements(), measurements);
9102         assertTrue(calibrator.isCommonAxisUsed());
9103         assertSame(calibrator.getListener(), this);
9104         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
9105         assertFalse(calibrator.isReady());
9106         assertFalse(calibrator.isRunning());
9107         assertNull(calibrator.getEstimatedBiases());
9108         assertFalse(calibrator.getEstimatedBiases(null));
9109         assertNull(calibrator.getEstimatedBiasesAsMatrix());
9110         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9111         assertNull(calibrator.getEstimatedBiasFx());
9112         assertNull(calibrator.getEstimatedBiasFy());
9113         assertNull(calibrator.getEstimatedBiasFz());
9114         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9115         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9116         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9117         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9118         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9119         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9120         assertNull(calibrator.getEstimatedMa());
9121         assertNull(calibrator.getEstimatedSx());
9122         assertNull(calibrator.getEstimatedSy());
9123         assertNull(calibrator.getEstimatedSz());
9124         assertNull(calibrator.getEstimatedMxy());
9125         assertNull(calibrator.getEstimatedMxz());
9126         assertNull(calibrator.getEstimatedMyx());
9127         assertNull(calibrator.getEstimatedMyz());
9128         assertNull(calibrator.getEstimatedMzx());
9129         assertNull(calibrator.getEstimatedMzy());
9130         assertNull(calibrator.getEstimatedCovariance());
9131         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9132         assertNull(calibrator.getGroundTruthGravityNorm());
9133         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9134         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
9135 
9136         // Force IllegalArgumentException
9137         calibrator = null;
9138         try {
9139             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
9140                     true, new Matrix(1, 1), ma,
9141                     this);
9142             fail("IllegalArgumentException expected but not thrown");
9143         } catch (final IllegalArgumentException ignore) {
9144         }
9145         try {
9146             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
9147                     true, new Matrix(1, 3), ma,
9148                     this);
9149             fail("IllegalArgumentException expected but not thrown");
9150         } catch (final IllegalArgumentException ignore) {
9151         }
9152         try {
9153             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
9154                     true, ba, new Matrix(1, 3),
9155                     this);
9156             fail("IllegalArgumentException expected but not thrown");
9157         } catch (final IllegalArgumentException ignore) {
9158         }
9159         try {
9160             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
9161                     true, ba, new Matrix(3, 1),
9162                     this);
9163             fail("IllegalArgumentException expected but not thrown");
9164         } catch (final IllegalArgumentException ignore) {
9165         }
9166         assertNull(calibrator);
9167     }
9168 
9169     @Test
9170     public void testConstructor79() throws WrongSizeException {
9171         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9172         final double latitude = Math.toRadians(
9173                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9174         final double longitude = Math.toRadians(
9175                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9176         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9177         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9178         final NEDVelocity nedVelocity = new NEDVelocity();
9179         final ECEFPosition ecefPosition = new ECEFPosition();
9180         final ECEFVelocity ecefVelocity = new ECEFVelocity();
9181         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9182                 ecefPosition, ecefVelocity);
9183 
9184         final KnownPositionAccelerometerCalibrator calibrator =
9185                 new KnownPositionAccelerometerCalibrator(ecefPosition);
9186 
9187         // check default values
9188         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9189         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9190         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9191         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9192         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9193         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9194         final Acceleration bx2 = new Acceleration(0.0,
9195                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9196         calibrator.getInitialBiasXAsAcceleration(bx2);
9197         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9198         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9199         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9200         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9201         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9202         final Acceleration by2 = new Acceleration(0.0,
9203                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9204         calibrator.getInitialBiasYAsAcceleration(by2);
9205         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9206         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9207         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9208         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9209         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9210         final Acceleration bz2 = new Acceleration(0.0,
9211                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9212         calibrator.getInitialBiasZAsAcceleration(bz2);
9213         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9214         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9215         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9216         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9217         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9218         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9219         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9220         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9221         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9222         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9223         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9224         final double[] bias1 = calibrator.getInitialBias();
9225         assertArrayEquals(bias1, new double[3], 0.0);
9226         final double[] bias2 = new double[3];
9227         calibrator.getInitialBias(bias2);
9228         assertArrayEquals(bias1, bias2, 0.0);
9229         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9230         assertEquals(b1, new Matrix(3, 1));
9231         final Matrix b2 = new Matrix(3, 1);
9232         calibrator.getInitialBiasAsMatrix(b2);
9233         assertEquals(b1, b2);
9234         final Matrix ma1 = calibrator.getInitialMa();
9235         assertEquals(ma1, new Matrix(3, 3));
9236         final Matrix ma2 = new Matrix(3, 3);
9237         calibrator.getInitialMa(ma2);
9238         assertEquals(ma1, ma2);
9239         assertSame(calibrator.getEcefPosition(), ecefPosition);
9240         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9241         final NEDPosition nedPosition1 = new NEDPosition();
9242         assertTrue(calibrator.getNedPosition(nedPosition1));
9243         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9244         assertNull(calibrator.getMeasurements());
9245         assertFalse(calibrator.isCommonAxisUsed());
9246         assertNull(calibrator.getListener());
9247         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
9248         assertFalse(calibrator.isReady());
9249         assertFalse(calibrator.isRunning());
9250         assertNull(calibrator.getEstimatedBiases());
9251         assertFalse(calibrator.getEstimatedBiases(null));
9252         assertNull(calibrator.getEstimatedBiasesAsMatrix());
9253         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9254         assertNull(calibrator.getEstimatedBiasFx());
9255         assertNull(calibrator.getEstimatedBiasFy());
9256         assertNull(calibrator.getEstimatedBiasFz());
9257         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9258         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9259         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9260         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9261         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9262         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9263         assertNull(calibrator.getEstimatedMa());
9264         assertNull(calibrator.getEstimatedSx());
9265         assertNull(calibrator.getEstimatedSy());
9266         assertNull(calibrator.getEstimatedSz());
9267         assertNull(calibrator.getEstimatedMxy());
9268         assertNull(calibrator.getEstimatedMxz());
9269         assertNull(calibrator.getEstimatedMyx());
9270         assertNull(calibrator.getEstimatedMyz());
9271         assertNull(calibrator.getEstimatedMzx());
9272         assertNull(calibrator.getEstimatedMzy());
9273         assertNull(calibrator.getEstimatedCovariance());
9274         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9275         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9276                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9277         assertNotNull(calibrator.getGroundTruthGravityNorm());
9278         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9279         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9280         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9281                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9282         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9283         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9284         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9285     }
9286 
9287     @Test
9288     public void testConstructor80() throws WrongSizeException {
9289         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9290         final double latitude = Math.toRadians(
9291                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9292         final double longitude = Math.toRadians(
9293                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9294         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9295         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9296         final NEDVelocity nedVelocity = new NEDVelocity();
9297         final ECEFPosition ecefPosition = new ECEFPosition();
9298         final ECEFVelocity ecefVelocity = new ECEFVelocity();
9299         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9300                 ecefPosition, ecefVelocity);
9301 
9302         final KnownPositionAccelerometerCalibrator calibrator =
9303                 new KnownPositionAccelerometerCalibrator(ecefPosition, this);
9304 
9305         // check default values
9306         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9307         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9308         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9309         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9310         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9311         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9312         final Acceleration bx2 = new Acceleration(0.0,
9313                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9314         calibrator.getInitialBiasXAsAcceleration(bx2);
9315         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9316         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9317         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9318         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9319         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9320         final Acceleration by2 = new Acceleration(0.0,
9321                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9322         calibrator.getInitialBiasYAsAcceleration(by2);
9323         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9324         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9325         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9326         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9327         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9328         final Acceleration bz2 = new Acceleration(0.0,
9329                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9330         calibrator.getInitialBiasZAsAcceleration(bz2);
9331         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9332         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9333         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9334         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9335         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9336         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9337         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9338         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9339         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9340         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9341         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9342         final double[] bias1 = calibrator.getInitialBias();
9343         assertArrayEquals(bias1, new double[3], 0.0);
9344         final double[] bias2 = new double[3];
9345         calibrator.getInitialBias(bias2);
9346         assertArrayEquals(bias1, bias2, 0.0);
9347         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9348         assertEquals(b1, new Matrix(3, 1));
9349         final Matrix b2 = new Matrix(3, 1);
9350         calibrator.getInitialBiasAsMatrix(b2);
9351         assertEquals(b1, b2);
9352         final Matrix ma1 = calibrator.getInitialMa();
9353         assertEquals(ma1, new Matrix(3, 3));
9354         final Matrix ma2 = new Matrix(3, 3);
9355         calibrator.getInitialMa(ma2);
9356         assertEquals(ma1, ma2);
9357         assertSame(calibrator.getEcefPosition(), ecefPosition);
9358         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9359         final NEDPosition nedPosition1 = new NEDPosition();
9360         assertTrue(calibrator.getNedPosition(nedPosition1));
9361         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9362         assertNull(calibrator.getMeasurements());
9363         assertFalse(calibrator.isCommonAxisUsed());
9364         assertSame(calibrator.getListener(), this);
9365         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
9366         assertFalse(calibrator.isReady());
9367         assertFalse(calibrator.isRunning());
9368         assertNull(calibrator.getEstimatedBiases());
9369         assertFalse(calibrator.getEstimatedBiases(null));
9370         assertNull(calibrator.getEstimatedBiasesAsMatrix());
9371         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9372         assertNull(calibrator.getEstimatedBiasFx());
9373         assertNull(calibrator.getEstimatedBiasFy());
9374         assertNull(calibrator.getEstimatedBiasFz());
9375         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9376         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9377         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9378         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9379         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9380         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9381         assertNull(calibrator.getEstimatedMa());
9382         assertNull(calibrator.getEstimatedSx());
9383         assertNull(calibrator.getEstimatedSy());
9384         assertNull(calibrator.getEstimatedSz());
9385         assertNull(calibrator.getEstimatedMxy());
9386         assertNull(calibrator.getEstimatedMxz());
9387         assertNull(calibrator.getEstimatedMyx());
9388         assertNull(calibrator.getEstimatedMyz());
9389         assertNull(calibrator.getEstimatedMzx());
9390         assertNull(calibrator.getEstimatedMzy());
9391         assertNull(calibrator.getEstimatedCovariance());
9392         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9393         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9394                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9395         assertNotNull(calibrator.getGroundTruthGravityNorm());
9396         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9397         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9398         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9399                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9400         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9401         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9402         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9403     }
9404 
9405     @Test
9406     public void testConstructor81() throws WrongSizeException {
9407         final Collection<StandardDeviationBodyKinematics> measurements =
9408                 Collections.emptyList();
9409 
9410         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9411         final double latitude = Math.toRadians(
9412                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9413         final double longitude = Math.toRadians(
9414                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9415         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9416         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9417         final NEDVelocity nedVelocity = new NEDVelocity();
9418         final ECEFPosition ecefPosition = new ECEFPosition();
9419         final ECEFVelocity ecefVelocity = new ECEFVelocity();
9420         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9421                 ecefPosition, ecefVelocity);
9422 
9423         final KnownPositionAccelerometerCalibrator calibrator =
9424                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements);
9425 
9426         // check default values
9427         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9428         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9429         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9430         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9431         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9432         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9433         final Acceleration bx2 = new Acceleration(0.0,
9434                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9435         calibrator.getInitialBiasXAsAcceleration(bx2);
9436         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9437         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9438         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9439         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9440         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9441         final Acceleration by2 = new Acceleration(0.0,
9442                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9443         calibrator.getInitialBiasYAsAcceleration(by2);
9444         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9445         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9446         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9447         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9448         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9449         final Acceleration bz2 = new Acceleration(0.0,
9450                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9451         calibrator.getInitialBiasZAsAcceleration(bz2);
9452         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9453         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9454         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9455         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9456         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9457         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9458         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9459         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9460         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9461         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9462         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9463         final double[] bias1 = calibrator.getInitialBias();
9464         assertArrayEquals(bias1, new double[3], 0.0);
9465         final double[] bias2 = new double[3];
9466         calibrator.getInitialBias(bias2);
9467         assertArrayEquals(bias1, bias2, 0.0);
9468         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9469         assertEquals(b1, new Matrix(3, 1));
9470         final Matrix b2 = new Matrix(3, 1);
9471         calibrator.getInitialBiasAsMatrix(b2);
9472         assertEquals(b1, b2);
9473         final Matrix ma1 = calibrator.getInitialMa();
9474         assertEquals(ma1, new Matrix(3, 3));
9475         final Matrix ma2 = new Matrix(3, 3);
9476         calibrator.getInitialMa(ma2);
9477         assertEquals(ma1, ma2);
9478         assertSame(calibrator.getEcefPosition(), ecefPosition);
9479         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9480         final NEDPosition nedPosition1 = new NEDPosition();
9481         assertTrue(calibrator.getNedPosition(nedPosition1));
9482         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9483         assertSame(calibrator.getMeasurements(), measurements);
9484         assertFalse(calibrator.isCommonAxisUsed());
9485         assertNull(calibrator.getListener());
9486         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
9487         assertFalse(calibrator.isReady());
9488         assertFalse(calibrator.isRunning());
9489         assertNull(calibrator.getEstimatedBiases());
9490         assertFalse(calibrator.getEstimatedBiases(null));
9491         assertNull(calibrator.getEstimatedBiasesAsMatrix());
9492         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9493         assertNull(calibrator.getEstimatedBiasFx());
9494         assertNull(calibrator.getEstimatedBiasFy());
9495         assertNull(calibrator.getEstimatedBiasFz());
9496         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9497         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9498         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9499         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9500         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9501         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9502         assertNull(calibrator.getEstimatedMa());
9503         assertNull(calibrator.getEstimatedSx());
9504         assertNull(calibrator.getEstimatedSy());
9505         assertNull(calibrator.getEstimatedSz());
9506         assertNull(calibrator.getEstimatedMxy());
9507         assertNull(calibrator.getEstimatedMxz());
9508         assertNull(calibrator.getEstimatedMyx());
9509         assertNull(calibrator.getEstimatedMyz());
9510         assertNull(calibrator.getEstimatedMzx());
9511         assertNull(calibrator.getEstimatedMzy());
9512         assertNull(calibrator.getEstimatedCovariance());
9513         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9514         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9515                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9516         assertNotNull(calibrator.getGroundTruthGravityNorm());
9517         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9518         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9519         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9520                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9521         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9522         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9523         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9524     }
9525 
9526     @Test
9527     public void testConstructor82() throws WrongSizeException {
9528         final Collection<StandardDeviationBodyKinematics> measurements =
9529                 Collections.emptyList();
9530 
9531         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9532         final double latitude = Math.toRadians(
9533                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9534         final double longitude = Math.toRadians(
9535                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9536         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9537         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9538         final NEDVelocity nedVelocity = new NEDVelocity();
9539         final ECEFPosition ecefPosition = new ECEFPosition();
9540         final ECEFVelocity ecefVelocity = new ECEFVelocity();
9541         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9542                 ecefPosition, ecefVelocity);
9543 
9544         final KnownPositionAccelerometerCalibrator calibrator =
9545                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
9546                         this);
9547 
9548         // check default values
9549         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9550         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9551         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9552         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9553         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9554         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9555         final Acceleration bx2 = new Acceleration(0.0,
9556                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9557         calibrator.getInitialBiasXAsAcceleration(bx2);
9558         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9559         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9560         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9561         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9562         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9563         final Acceleration by2 = new Acceleration(0.0,
9564                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9565         calibrator.getInitialBiasYAsAcceleration(by2);
9566         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9567         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9568         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9569         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9570         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9571         final Acceleration bz2 = new Acceleration(0.0,
9572                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9573         calibrator.getInitialBiasZAsAcceleration(bz2);
9574         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9575         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9576         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9577         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9578         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9579         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9580         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9581         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9582         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9583         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9584         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9585         final double[] bias1 = calibrator.getInitialBias();
9586         assertArrayEquals(bias1, new double[3], 0.0);
9587         final double[] bias2 = new double[3];
9588         calibrator.getInitialBias(bias2);
9589         assertArrayEquals(bias1, bias2, 0.0);
9590         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9591         assertEquals(b1, new Matrix(3, 1));
9592         final Matrix b2 = new Matrix(3, 1);
9593         calibrator.getInitialBiasAsMatrix(b2);
9594         assertEquals(b1, b2);
9595         final Matrix ma1 = calibrator.getInitialMa();
9596         assertEquals(ma1, new Matrix(3, 3));
9597         final Matrix ma2 = new Matrix(3, 3);
9598         calibrator.getInitialMa(ma2);
9599         assertEquals(ma1, ma2);
9600         assertSame(calibrator.getEcefPosition(), ecefPosition);
9601         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9602         final NEDPosition nedPosition1 = new NEDPosition();
9603         assertTrue(calibrator.getNedPosition(nedPosition1));
9604         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9605         assertSame(calibrator.getMeasurements(), measurements);
9606         assertFalse(calibrator.isCommonAxisUsed());
9607         assertSame(calibrator.getListener(), this);
9608         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
9609         assertFalse(calibrator.isReady());
9610         assertFalse(calibrator.isRunning());
9611         assertNull(calibrator.getEstimatedBiases());
9612         assertFalse(calibrator.getEstimatedBiases(null));
9613         assertNull(calibrator.getEstimatedBiasesAsMatrix());
9614         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9615         assertNull(calibrator.getEstimatedBiasFx());
9616         assertNull(calibrator.getEstimatedBiasFy());
9617         assertNull(calibrator.getEstimatedBiasFz());
9618         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9619         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9620         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9621         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9622         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9623         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9624         assertNull(calibrator.getEstimatedMa());
9625         assertNull(calibrator.getEstimatedSx());
9626         assertNull(calibrator.getEstimatedSy());
9627         assertNull(calibrator.getEstimatedSz());
9628         assertNull(calibrator.getEstimatedMxy());
9629         assertNull(calibrator.getEstimatedMxz());
9630         assertNull(calibrator.getEstimatedMyx());
9631         assertNull(calibrator.getEstimatedMyz());
9632         assertNull(calibrator.getEstimatedMzx());
9633         assertNull(calibrator.getEstimatedMzy());
9634         assertNull(calibrator.getEstimatedCovariance());
9635         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9636         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9637                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9638         assertNotNull(calibrator.getGroundTruthGravityNorm());
9639         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9640         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9641         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9642                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9643         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9644         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9645         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9646     }
9647 
9648     @Test
9649     public void testConstructor83() throws WrongSizeException {
9650         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9651         final double latitude = Math.toRadians(
9652                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9653         final double longitude = Math.toRadians(
9654                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9655         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9656         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9657         final NEDVelocity nedVelocity = new NEDVelocity();
9658         final ECEFPosition ecefPosition = new ECEFPosition();
9659         final ECEFVelocity ecefVelocity = new ECEFVelocity();
9660         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9661                 ecefPosition, ecefVelocity);
9662 
9663         final KnownPositionAccelerometerCalibrator calibrator =
9664                 new KnownPositionAccelerometerCalibrator(ecefPosition,
9665                         true);
9666 
9667         // check default values
9668         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9669         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9670         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9671         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9672         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9673         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9674         final Acceleration bx2 = new Acceleration(0.0,
9675                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9676         calibrator.getInitialBiasXAsAcceleration(bx2);
9677         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9678         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9679         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9680         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9681         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9682         final Acceleration by2 = new Acceleration(0.0,
9683                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9684         calibrator.getInitialBiasYAsAcceleration(by2);
9685         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9686         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9687         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9688         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9689         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9690         final Acceleration bz2 = new Acceleration(0.0,
9691                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9692         calibrator.getInitialBiasZAsAcceleration(bz2);
9693         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9694         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9695         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9696         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9697         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9698         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9699         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9700         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9701         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9702         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9703         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9704         final double[] bias1 = calibrator.getInitialBias();
9705         assertArrayEquals(bias1, new double[3], 0.0);
9706         final double[] bias2 = new double[3];
9707         calibrator.getInitialBias(bias2);
9708         assertArrayEquals(bias1, bias2, 0.0);
9709         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9710         assertEquals(b1, new Matrix(3, 1));
9711         final Matrix b2 = new Matrix(3, 1);
9712         calibrator.getInitialBiasAsMatrix(b2);
9713         assertEquals(b1, b2);
9714         final Matrix ma1 = calibrator.getInitialMa();
9715         assertEquals(ma1, new Matrix(3, 3));
9716         final Matrix ma2 = new Matrix(3, 3);
9717         calibrator.getInitialMa(ma2);
9718         assertEquals(ma1, ma2);
9719         assertSame(calibrator.getEcefPosition(), ecefPosition);
9720         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9721         final NEDPosition nedPosition1 = new NEDPosition();
9722         assertTrue(calibrator.getNedPosition(nedPosition1));
9723         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9724         assertNull(calibrator.getMeasurements());
9725         assertTrue(calibrator.isCommonAxisUsed());
9726         assertNull(calibrator.getListener());
9727         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
9728         assertFalse(calibrator.isReady());
9729         assertFalse(calibrator.isRunning());
9730         assertNull(calibrator.getEstimatedBiases());
9731         assertFalse(calibrator.getEstimatedBiases(null));
9732         assertNull(calibrator.getEstimatedBiasesAsMatrix());
9733         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9734         assertNull(calibrator.getEstimatedBiasFx());
9735         assertNull(calibrator.getEstimatedBiasFy());
9736         assertNull(calibrator.getEstimatedBiasFz());
9737         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9738         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9739         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9740         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9741         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9742         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9743         assertNull(calibrator.getEstimatedMa());
9744         assertNull(calibrator.getEstimatedSx());
9745         assertNull(calibrator.getEstimatedSy());
9746         assertNull(calibrator.getEstimatedSz());
9747         assertNull(calibrator.getEstimatedMxy());
9748         assertNull(calibrator.getEstimatedMxz());
9749         assertNull(calibrator.getEstimatedMyx());
9750         assertNull(calibrator.getEstimatedMyz());
9751         assertNull(calibrator.getEstimatedMzx());
9752         assertNull(calibrator.getEstimatedMzy());
9753         assertNull(calibrator.getEstimatedCovariance());
9754         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9755         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9756                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9757         assertNotNull(calibrator.getGroundTruthGravityNorm());
9758         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9759         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9760         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9761                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9762         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9763         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9764         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9765     }
9766 
9767     @Test
9768     public void testConstructor84() throws WrongSizeException {
9769         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9770         final double latitude = Math.toRadians(
9771                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9772         final double longitude = Math.toRadians(
9773                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9774         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9775         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9776         final NEDVelocity nedVelocity = new NEDVelocity();
9777         final ECEFPosition ecefPosition = new ECEFPosition();
9778         final ECEFVelocity ecefVelocity = new ECEFVelocity();
9779         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9780                 ecefPosition, ecefVelocity);
9781 
9782         final KnownPositionAccelerometerCalibrator calibrator =
9783                 new KnownPositionAccelerometerCalibrator(ecefPosition,
9784                         true, this);
9785 
9786         // check default values
9787         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9788         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9789         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9790         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9791         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9792         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9793         final Acceleration bx2 = new Acceleration(0.0,
9794                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9795         calibrator.getInitialBiasXAsAcceleration(bx2);
9796         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9797         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9798         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9799         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9800         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9801         final Acceleration by2 = new Acceleration(0.0,
9802                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9803         calibrator.getInitialBiasYAsAcceleration(by2);
9804         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9805         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9806         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9807         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9808         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9809         final Acceleration bz2 = new Acceleration(0.0,
9810                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9811         calibrator.getInitialBiasZAsAcceleration(bz2);
9812         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9813         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9814         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9815         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9816         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9817         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9818         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9819         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9820         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9821         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9822         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9823         final double[] bias1 = calibrator.getInitialBias();
9824         assertArrayEquals(bias1, new double[3], 0.0);
9825         final double[] bias2 = new double[3];
9826         calibrator.getInitialBias(bias2);
9827         assertArrayEquals(bias1, bias2, 0.0);
9828         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9829         assertEquals(b1, new Matrix(3, 1));
9830         final Matrix b2 = new Matrix(3, 1);
9831         calibrator.getInitialBiasAsMatrix(b2);
9832         assertEquals(b1, b2);
9833         final Matrix ma1 = calibrator.getInitialMa();
9834         assertEquals(ma1, new Matrix(3, 3));
9835         final Matrix ma2 = new Matrix(3, 3);
9836         calibrator.getInitialMa(ma2);
9837         assertEquals(ma1, ma2);
9838         assertSame(calibrator.getEcefPosition(), ecefPosition);
9839         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9840         final NEDPosition nedPosition1 = new NEDPosition();
9841         assertTrue(calibrator.getNedPosition(nedPosition1));
9842         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9843         assertNull(calibrator.getMeasurements());
9844         assertTrue(calibrator.isCommonAxisUsed());
9845         assertSame(calibrator.getListener(), this);
9846         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
9847         assertFalse(calibrator.isReady());
9848         assertFalse(calibrator.isRunning());
9849         assertNull(calibrator.getEstimatedBiases());
9850         assertFalse(calibrator.getEstimatedBiases(null));
9851         assertNull(calibrator.getEstimatedBiasesAsMatrix());
9852         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9853         assertNull(calibrator.getEstimatedBiasFx());
9854         assertNull(calibrator.getEstimatedBiasFy());
9855         assertNull(calibrator.getEstimatedBiasFz());
9856         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9857         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9858         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9859         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9860         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9861         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9862         assertNull(calibrator.getEstimatedMa());
9863         assertNull(calibrator.getEstimatedSx());
9864         assertNull(calibrator.getEstimatedSy());
9865         assertNull(calibrator.getEstimatedSz());
9866         assertNull(calibrator.getEstimatedMxy());
9867         assertNull(calibrator.getEstimatedMxz());
9868         assertNull(calibrator.getEstimatedMyx());
9869         assertNull(calibrator.getEstimatedMyz());
9870         assertNull(calibrator.getEstimatedMzx());
9871         assertNull(calibrator.getEstimatedMzy());
9872         assertNull(calibrator.getEstimatedCovariance());
9873         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9874         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9875                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9876         assertNotNull(calibrator.getGroundTruthGravityNorm());
9877         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9878         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9879         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9880                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9881         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9882         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9883         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9884     }
9885 
9886     @Test
9887     public void testConstructor85() throws WrongSizeException {
9888         final Collection<StandardDeviationBodyKinematics> measurements =
9889                 Collections.emptyList();
9890         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9891         final double latitude = Math.toRadians(
9892                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9893         final double longitude = Math.toRadians(
9894                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9895         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9896         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9897         final NEDVelocity nedVelocity = new NEDVelocity();
9898         final ECEFPosition ecefPosition = new ECEFPosition();
9899         final ECEFVelocity ecefVelocity = new ECEFVelocity();
9900         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9901                 ecefPosition, ecefVelocity);
9902 
9903         final KnownPositionAccelerometerCalibrator calibrator =
9904                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
9905                         true);
9906 
9907         // check default values
9908         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9909         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9910         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9911         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9912         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9913         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9914         final Acceleration bx2 = new Acceleration(0.0,
9915                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9916         calibrator.getInitialBiasXAsAcceleration(bx2);
9917         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9918         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9919         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9920         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9921         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9922         final Acceleration by2 = new Acceleration(0.0,
9923                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9924         calibrator.getInitialBiasYAsAcceleration(by2);
9925         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9926         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9927         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9928         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9929         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9930         final Acceleration bz2 = new Acceleration(0.0,
9931                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9932         calibrator.getInitialBiasZAsAcceleration(bz2);
9933         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9934         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9935         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9936         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9937         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9938         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9939         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9940         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9941         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9942         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9943         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9944         final double[] bias1 = calibrator.getInitialBias();
9945         assertArrayEquals(bias1, new double[3], 0.0);
9946         final double[] bias2 = new double[3];
9947         calibrator.getInitialBias(bias2);
9948         assertArrayEquals(bias1, bias2, 0.0);
9949         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9950         assertEquals(b1, new Matrix(3, 1));
9951         final Matrix b2 = new Matrix(3, 1);
9952         calibrator.getInitialBiasAsMatrix(b2);
9953         assertEquals(b1, b2);
9954         final Matrix ma1 = calibrator.getInitialMa();
9955         assertEquals(ma1, new Matrix(3, 3));
9956         final Matrix ma2 = new Matrix(3, 3);
9957         calibrator.getInitialMa(ma2);
9958         assertEquals(ma1, ma2);
9959         assertSame(calibrator.getEcefPosition(), ecefPosition);
9960         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9961         final NEDPosition nedPosition1 = new NEDPosition();
9962         assertTrue(calibrator.getNedPosition(nedPosition1));
9963         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9964         assertSame(calibrator.getMeasurements(), measurements);
9965         assertTrue(calibrator.isCommonAxisUsed());
9966         assertNull(calibrator.getListener());
9967         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
9968         assertFalse(calibrator.isReady());
9969         assertFalse(calibrator.isRunning());
9970         assertNull(calibrator.getEstimatedBiases());
9971         assertFalse(calibrator.getEstimatedBiases(null));
9972         assertNull(calibrator.getEstimatedBiasesAsMatrix());
9973         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9974         assertNull(calibrator.getEstimatedBiasFx());
9975         assertNull(calibrator.getEstimatedBiasFy());
9976         assertNull(calibrator.getEstimatedBiasFz());
9977         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9978         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9979         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9980         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9981         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9982         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9983         assertNull(calibrator.getEstimatedMa());
9984         assertNull(calibrator.getEstimatedSx());
9985         assertNull(calibrator.getEstimatedSy());
9986         assertNull(calibrator.getEstimatedSz());
9987         assertNull(calibrator.getEstimatedMxy());
9988         assertNull(calibrator.getEstimatedMxz());
9989         assertNull(calibrator.getEstimatedMyx());
9990         assertNull(calibrator.getEstimatedMyz());
9991         assertNull(calibrator.getEstimatedMzx());
9992         assertNull(calibrator.getEstimatedMzy());
9993         assertNull(calibrator.getEstimatedCovariance());
9994         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9995         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9996                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9997         assertNotNull(calibrator.getGroundTruthGravityNorm());
9998         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9999         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10000         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10001                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10002         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10003         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10004         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10005     }
10006 
10007     @Test
10008     public void testConstructor86() throws WrongSizeException {
10009         final Collection<StandardDeviationBodyKinematics> measurements =
10010                 Collections.emptyList();
10011         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10012         final double latitude = Math.toRadians(
10013                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10014         final double longitude = Math.toRadians(
10015                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10016         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10017         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10018         final NEDVelocity nedVelocity = new NEDVelocity();
10019         final ECEFPosition ecefPosition = new ECEFPosition();
10020         final ECEFVelocity ecefVelocity = new ECEFVelocity();
10021         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10022                 ecefPosition, ecefVelocity);
10023 
10024         final KnownPositionAccelerometerCalibrator calibrator =
10025                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
10026                         true, this);
10027 
10028         // check default values
10029         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
10030         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
10031         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
10032         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10033         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
10034         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10035         final Acceleration bx2 = new Acceleration(0.0,
10036                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10037         calibrator.getInitialBiasXAsAcceleration(bx2);
10038         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
10039         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10040         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10041         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
10042         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10043         final Acceleration by2 = new Acceleration(0.0,
10044                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10045         calibrator.getInitialBiasYAsAcceleration(by2);
10046         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
10047         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10048         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10049         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
10050         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10051         final Acceleration bz2 = new Acceleration(0.0,
10052                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10053         calibrator.getInitialBiasZAsAcceleration(bz2);
10054         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
10055         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10056         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10057         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10058         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10059         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10060         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10061         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10062         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10063         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10064         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10065         final double[] bias1 = calibrator.getInitialBias();
10066         assertArrayEquals(bias1, new double[3], 0.0);
10067         final double[] bias2 = new double[3];
10068         calibrator.getInitialBias(bias2);
10069         assertArrayEquals(bias1, bias2, 0.0);
10070         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10071         assertEquals(b1, new Matrix(3, 1));
10072         final Matrix b2 = new Matrix(3, 1);
10073         calibrator.getInitialBiasAsMatrix(b2);
10074         assertEquals(b1, b2);
10075         final Matrix ma1 = calibrator.getInitialMa();
10076         assertEquals(ma1, new Matrix(3, 3));
10077         final Matrix ma2 = new Matrix(3, 3);
10078         calibrator.getInitialMa(ma2);
10079         assertEquals(ma1, ma2);
10080         assertSame(calibrator.getEcefPosition(), ecefPosition);
10081         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10082         final NEDPosition nedPosition1 = new NEDPosition();
10083         assertTrue(calibrator.getNedPosition(nedPosition1));
10084         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10085         assertSame(calibrator.getMeasurements(), measurements);
10086         assertTrue(calibrator.isCommonAxisUsed());
10087         assertSame(calibrator.getListener(), this);
10088         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
10089         assertFalse(calibrator.isReady());
10090         assertFalse(calibrator.isRunning());
10091         assertNull(calibrator.getEstimatedBiases());
10092         assertFalse(calibrator.getEstimatedBiases(null));
10093         assertNull(calibrator.getEstimatedBiasesAsMatrix());
10094         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10095         assertNull(calibrator.getEstimatedBiasFx());
10096         assertNull(calibrator.getEstimatedBiasFy());
10097         assertNull(calibrator.getEstimatedBiasFz());
10098         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10099         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10100         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10101         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10102         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10103         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10104         assertNull(calibrator.getEstimatedMa());
10105         assertNull(calibrator.getEstimatedSx());
10106         assertNull(calibrator.getEstimatedSy());
10107         assertNull(calibrator.getEstimatedSz());
10108         assertNull(calibrator.getEstimatedMxy());
10109         assertNull(calibrator.getEstimatedMxz());
10110         assertNull(calibrator.getEstimatedMyx());
10111         assertNull(calibrator.getEstimatedMyz());
10112         assertNull(calibrator.getEstimatedMzx());
10113         assertNull(calibrator.getEstimatedMzy());
10114         assertNull(calibrator.getEstimatedCovariance());
10115         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10116         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10117                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10118         assertNotNull(calibrator.getGroundTruthGravityNorm());
10119         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10120         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10121         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10122                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10123         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10124         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10125         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10126     }
10127 
10128     @Test
10129     public void testConstructor87() throws WrongSizeException {
10130         final Matrix ba = generateBa();
10131         final double biasX = ba.getElementAtIndex(0);
10132         final double biasY = ba.getElementAtIndex(1);
10133         final double biasZ = ba.getElementAtIndex(2);
10134 
10135         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10136         final double latitude = Math.toRadians(
10137                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10138         final double longitude = Math.toRadians(
10139                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10140         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10141         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10142         final NEDVelocity nedVelocity = new NEDVelocity();
10143         final ECEFPosition ecefPosition = new ECEFPosition();
10144         final ECEFVelocity ecefVelocity = new ECEFVelocity();
10145         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10146                 ecefPosition, ecefVelocity);
10147 
10148         final KnownPositionAccelerometerCalibrator calibrator =
10149                 new KnownPositionAccelerometerCalibrator(ecefPosition,
10150                         biasX, biasY, biasZ);
10151 
10152         // check default values
10153         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10154         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10155         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10156         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10157         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10158         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10159         final Acceleration bx2 = new Acceleration(0.0,
10160                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10161         calibrator.getInitialBiasXAsAcceleration(bx2);
10162         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10163         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10164         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10165         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10166         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10167         final Acceleration by2 = new Acceleration(0.0,
10168                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10169         calibrator.getInitialBiasYAsAcceleration(by2);
10170         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10171         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10172         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10173         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10174         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10175         final Acceleration bz2 = new Acceleration(0.0,
10176                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10177         calibrator.getInitialBiasZAsAcceleration(bz2);
10178         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10179         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10180         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10181         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10182         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10183         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10184         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10185         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10186         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10187         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10188         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10189         final double[] bias1 = calibrator.getInitialBias();
10190         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10191         final double[] bias2 = new double[3];
10192         calibrator.getInitialBias(bias2);
10193         assertArrayEquals(bias1, bias2, 0.0);
10194         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10195         assertEquals(b1, ba);
10196         final Matrix b2 = new Matrix(3, 1);
10197         calibrator.getInitialBiasAsMatrix(b2);
10198         assertEquals(b1, b2);
10199         final Matrix ma1 = calibrator.getInitialMa();
10200         assertEquals(ma1, new Matrix(3, 3));
10201         final Matrix ma2 = new Matrix(3, 3);
10202         calibrator.getInitialMa(ma2);
10203         assertEquals(ma1, ma2);
10204         assertSame(calibrator.getEcefPosition(), ecefPosition);
10205         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10206         final NEDPosition nedPosition1 = new NEDPosition();
10207         assertTrue(calibrator.getNedPosition(nedPosition1));
10208         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10209         assertNull(calibrator.getMeasurements());
10210         assertFalse(calibrator.isCommonAxisUsed());
10211         assertNull(calibrator.getListener());
10212         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
10213         assertFalse(calibrator.isReady());
10214         assertFalse(calibrator.isRunning());
10215         assertNull(calibrator.getEstimatedBiases());
10216         assertFalse(calibrator.getEstimatedBiases(null));
10217         assertNull(calibrator.getEstimatedBiasesAsMatrix());
10218         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10219         assertNull(calibrator.getEstimatedBiasFx());
10220         assertNull(calibrator.getEstimatedBiasFy());
10221         assertNull(calibrator.getEstimatedBiasFz());
10222         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10223         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10224         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10225         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10226         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10227         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10228         assertNull(calibrator.getEstimatedMa());
10229         assertNull(calibrator.getEstimatedSx());
10230         assertNull(calibrator.getEstimatedSy());
10231         assertNull(calibrator.getEstimatedSz());
10232         assertNull(calibrator.getEstimatedMxy());
10233         assertNull(calibrator.getEstimatedMxz());
10234         assertNull(calibrator.getEstimatedMyx());
10235         assertNull(calibrator.getEstimatedMyz());
10236         assertNull(calibrator.getEstimatedMzx());
10237         assertNull(calibrator.getEstimatedMzy());
10238         assertNull(calibrator.getEstimatedCovariance());
10239         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10240         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10241                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10242         assertNotNull(calibrator.getGroundTruthGravityNorm());
10243         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10244         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10245         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10246                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10247         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10248         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10249         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10250     }
10251 
10252     @Test
10253     public void testConstructor88() throws WrongSizeException {
10254         final Matrix ba = generateBa();
10255         final double biasX = ba.getElementAtIndex(0);
10256         final double biasY = ba.getElementAtIndex(1);
10257         final double biasZ = ba.getElementAtIndex(2);
10258 
10259         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10260         final double latitude = Math.toRadians(
10261                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10262         final double longitude = Math.toRadians(
10263                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10264         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10265         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10266         final NEDVelocity nedVelocity = new NEDVelocity();
10267         final ECEFPosition ecefPosition = new ECEFPosition();
10268         final ECEFVelocity ecefVelocity = new ECEFVelocity();
10269         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10270                 ecefPosition, ecefVelocity);
10271 
10272         final KnownPositionAccelerometerCalibrator calibrator =
10273                 new KnownPositionAccelerometerCalibrator(ecefPosition,
10274                         biasX, biasY, biasZ, this);
10275 
10276         // check default values
10277         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10278         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10279         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10280         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10281         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10282         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10283         final Acceleration bx2 = new Acceleration(0.0,
10284                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10285         calibrator.getInitialBiasXAsAcceleration(bx2);
10286         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10287         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10288         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10289         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10290         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10291         final Acceleration by2 = new Acceleration(0.0,
10292                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10293         calibrator.getInitialBiasYAsAcceleration(by2);
10294         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10295         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10296         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10297         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10298         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10299         final Acceleration bz2 = new Acceleration(0.0,
10300                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10301         calibrator.getInitialBiasZAsAcceleration(bz2);
10302         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10303         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10304         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10305         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10306         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10307         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10308         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10309         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10310         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10311         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10312         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10313         final double[] bias1 = calibrator.getInitialBias();
10314         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10315         final double[] bias2 = new double[3];
10316         calibrator.getInitialBias(bias2);
10317         assertArrayEquals(bias1, bias2, 0.0);
10318         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10319         assertEquals(b1, ba);
10320         final Matrix b2 = new Matrix(3, 1);
10321         calibrator.getInitialBiasAsMatrix(b2);
10322         assertEquals(b1, b2);
10323         final Matrix ma1 = calibrator.getInitialMa();
10324         assertEquals(ma1, new Matrix(3, 3));
10325         final Matrix ma2 = new Matrix(3, 3);
10326         calibrator.getInitialMa(ma2);
10327         assertEquals(ma1, ma2);
10328         assertSame(calibrator.getEcefPosition(), ecefPosition);
10329         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10330         final NEDPosition nedPosition1 = new NEDPosition();
10331         assertTrue(calibrator.getNedPosition(nedPosition1));
10332         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10333         assertNull(calibrator.getMeasurements());
10334         assertFalse(calibrator.isCommonAxisUsed());
10335         assertSame(calibrator.getListener(), this);
10336         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
10337         assertFalse(calibrator.isReady());
10338         assertFalse(calibrator.isRunning());
10339         assertNull(calibrator.getEstimatedBiases());
10340         assertFalse(calibrator.getEstimatedBiases(null));
10341         assertNull(calibrator.getEstimatedBiasesAsMatrix());
10342         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10343         assertNull(calibrator.getEstimatedBiasFx());
10344         assertNull(calibrator.getEstimatedBiasFy());
10345         assertNull(calibrator.getEstimatedBiasFz());
10346         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10347         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10348         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10349         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10350         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10351         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10352         assertNull(calibrator.getEstimatedMa());
10353         assertNull(calibrator.getEstimatedSx());
10354         assertNull(calibrator.getEstimatedSy());
10355         assertNull(calibrator.getEstimatedSz());
10356         assertNull(calibrator.getEstimatedMxy());
10357         assertNull(calibrator.getEstimatedMxz());
10358         assertNull(calibrator.getEstimatedMyx());
10359         assertNull(calibrator.getEstimatedMyz());
10360         assertNull(calibrator.getEstimatedMzx());
10361         assertNull(calibrator.getEstimatedMzy());
10362         assertNull(calibrator.getEstimatedCovariance());
10363         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10364         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10365                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10366         assertNotNull(calibrator.getGroundTruthGravityNorm());
10367         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10368         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10369         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10370                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10371         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10372         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10373         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10374     }
10375 
10376     @Test
10377     public void testConstructor89() throws WrongSizeException {
10378         final Collection<StandardDeviationBodyKinematics> measurements =
10379                 Collections.emptyList();
10380 
10381         final Matrix ba = generateBa();
10382         final double biasX = ba.getElementAtIndex(0);
10383         final double biasY = ba.getElementAtIndex(1);
10384         final double biasZ = ba.getElementAtIndex(2);
10385 
10386         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10387         final double latitude = Math.toRadians(
10388                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10389         final double longitude = Math.toRadians(
10390                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10391         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10392         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10393         final NEDVelocity nedVelocity = new NEDVelocity();
10394         final ECEFPosition ecefPosition = new ECEFPosition();
10395         final ECEFVelocity ecefVelocity = new ECEFVelocity();
10396         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10397                 ecefPosition, ecefVelocity);
10398 
10399         final KnownPositionAccelerometerCalibrator calibrator =
10400                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
10401                         biasX, biasY, biasZ);
10402 
10403         // check default values
10404         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10405         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10406         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10407         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10408         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10409         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10410         final Acceleration bx2 = new Acceleration(0.0,
10411                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10412         calibrator.getInitialBiasXAsAcceleration(bx2);
10413         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10414         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10415         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10416         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10417         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10418         final Acceleration by2 = new Acceleration(0.0,
10419                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10420         calibrator.getInitialBiasYAsAcceleration(by2);
10421         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10422         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10423         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10424         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10425         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10426         final Acceleration bz2 = new Acceleration(0.0,
10427                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10428         calibrator.getInitialBiasZAsAcceleration(bz2);
10429         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10430         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10431         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10432         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10433         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10434         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10435         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10436         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10437         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10438         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10439         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10440         final double[] bias1 = calibrator.getInitialBias();
10441         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10442         final double[] bias2 = new double[3];
10443         calibrator.getInitialBias(bias2);
10444         assertArrayEquals(bias1, bias2, 0.0);
10445         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10446         assertEquals(b1, ba);
10447         final Matrix b2 = new Matrix(3, 1);
10448         calibrator.getInitialBiasAsMatrix(b2);
10449         assertEquals(b1, b2);
10450         final Matrix ma1 = calibrator.getInitialMa();
10451         assertEquals(ma1, new Matrix(3, 3));
10452         final Matrix ma2 = new Matrix(3, 3);
10453         calibrator.getInitialMa(ma2);
10454         assertEquals(ma1, ma2);
10455         assertSame(calibrator.getEcefPosition(), ecefPosition);
10456         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10457         final NEDPosition nedPosition1 = new NEDPosition();
10458         assertTrue(calibrator.getNedPosition(nedPosition1));
10459         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10460         assertSame(calibrator.getMeasurements(), measurements);
10461         assertFalse(calibrator.isCommonAxisUsed());
10462         assertNull(calibrator.getListener());
10463         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
10464         assertFalse(calibrator.isReady());
10465         assertFalse(calibrator.isRunning());
10466         assertNull(calibrator.getEstimatedBiases());
10467         assertFalse(calibrator.getEstimatedBiases(null));
10468         assertNull(calibrator.getEstimatedBiasesAsMatrix());
10469         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10470         assertNull(calibrator.getEstimatedBiasFx());
10471         assertNull(calibrator.getEstimatedBiasFy());
10472         assertNull(calibrator.getEstimatedBiasFz());
10473         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10474         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10475         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10476         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10477         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10478         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10479         assertNull(calibrator.getEstimatedMa());
10480         assertNull(calibrator.getEstimatedSx());
10481         assertNull(calibrator.getEstimatedSy());
10482         assertNull(calibrator.getEstimatedSz());
10483         assertNull(calibrator.getEstimatedMxy());
10484         assertNull(calibrator.getEstimatedMxz());
10485         assertNull(calibrator.getEstimatedMyx());
10486         assertNull(calibrator.getEstimatedMyz());
10487         assertNull(calibrator.getEstimatedMzx());
10488         assertNull(calibrator.getEstimatedMzy());
10489         assertNull(calibrator.getEstimatedCovariance());
10490         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10491         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10492                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10493         assertNotNull(calibrator.getGroundTruthGravityNorm());
10494         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10495         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10496         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10497                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10498         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10499         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10500         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10501     }
10502 
10503     @Test
10504     public void testConstructor90() throws WrongSizeException {
10505         final Collection<StandardDeviationBodyKinematics> measurements =
10506                 Collections.emptyList();
10507 
10508         final Matrix ba = generateBa();
10509         final double biasX = ba.getElementAtIndex(0);
10510         final double biasY = ba.getElementAtIndex(1);
10511         final double biasZ = ba.getElementAtIndex(2);
10512 
10513         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10514         final double latitude = Math.toRadians(
10515                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10516         final double longitude = Math.toRadians(
10517                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10518         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10519         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10520         final NEDVelocity nedVelocity = new NEDVelocity();
10521         final ECEFPosition ecefPosition = new ECEFPosition();
10522         final ECEFVelocity ecefVelocity = new ECEFVelocity();
10523         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10524                 ecefPosition, ecefVelocity);
10525 
10526         final KnownPositionAccelerometerCalibrator calibrator =
10527                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
10528                         biasX, biasY, biasZ, this);
10529 
10530         // check default values
10531         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10532         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10533         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10534         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10535         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10536         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10537         final Acceleration bx2 = new Acceleration(0.0,
10538                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10539         calibrator.getInitialBiasXAsAcceleration(bx2);
10540         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10541         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10542         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10543         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10544         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10545         final Acceleration by2 = new Acceleration(0.0,
10546                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10547         calibrator.getInitialBiasYAsAcceleration(by2);
10548         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10549         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10550         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10551         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10552         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10553         final Acceleration bz2 = new Acceleration(0.0,
10554                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10555         calibrator.getInitialBiasZAsAcceleration(bz2);
10556         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10557         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10558         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10559         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10560         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10561         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10562         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10563         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10564         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10565         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10566         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10567         final double[] bias1 = calibrator.getInitialBias();
10568         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10569         final double[] bias2 = new double[3];
10570         calibrator.getInitialBias(bias2);
10571         assertArrayEquals(bias1, bias2, 0.0);
10572         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10573         assertEquals(b1, ba);
10574         final Matrix b2 = new Matrix(3, 1);
10575         calibrator.getInitialBiasAsMatrix(b2);
10576         assertEquals(b1, b2);
10577         final Matrix ma1 = calibrator.getInitialMa();
10578         assertEquals(ma1, new Matrix(3, 3));
10579         final Matrix ma2 = new Matrix(3, 3);
10580         calibrator.getInitialMa(ma2);
10581         assertEquals(ma1, ma2);
10582         assertSame(calibrator.getEcefPosition(), ecefPosition);
10583         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10584         final NEDPosition nedPosition1 = new NEDPosition();
10585         assertTrue(calibrator.getNedPosition(nedPosition1));
10586         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10587         assertSame(calibrator.getMeasurements(), measurements);
10588         assertFalse(calibrator.isCommonAxisUsed());
10589         assertSame(calibrator.getListener(), this);
10590         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
10591         assertFalse(calibrator.isReady());
10592         assertFalse(calibrator.isRunning());
10593         assertNull(calibrator.getEstimatedBiases());
10594         assertFalse(calibrator.getEstimatedBiases(null));
10595         assertNull(calibrator.getEstimatedBiasesAsMatrix());
10596         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10597         assertNull(calibrator.getEstimatedBiasFx());
10598         assertNull(calibrator.getEstimatedBiasFy());
10599         assertNull(calibrator.getEstimatedBiasFz());
10600         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10601         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10602         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10603         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10604         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10605         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10606         assertNull(calibrator.getEstimatedMa());
10607         assertNull(calibrator.getEstimatedSx());
10608         assertNull(calibrator.getEstimatedSy());
10609         assertNull(calibrator.getEstimatedSz());
10610         assertNull(calibrator.getEstimatedMxy());
10611         assertNull(calibrator.getEstimatedMxz());
10612         assertNull(calibrator.getEstimatedMyx());
10613         assertNull(calibrator.getEstimatedMyz());
10614         assertNull(calibrator.getEstimatedMzx());
10615         assertNull(calibrator.getEstimatedMzy());
10616         assertNull(calibrator.getEstimatedCovariance());
10617         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10618         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10619                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10620         assertNotNull(calibrator.getGroundTruthGravityNorm());
10621         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10622         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10623         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10624                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10625         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10626         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10627         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10628     }
10629 
10630     @Test
10631     public void testConstructor91() throws WrongSizeException {
10632         final Matrix ba = generateBa();
10633         final double biasX = ba.getElementAtIndex(0);
10634         final double biasY = ba.getElementAtIndex(1);
10635         final double biasZ = ba.getElementAtIndex(2);
10636 
10637         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10638         final double latitude = Math.toRadians(
10639                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10640         final double longitude = Math.toRadians(
10641                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10642         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10643         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10644         final NEDVelocity nedVelocity = new NEDVelocity();
10645         final ECEFPosition ecefPosition = new ECEFPosition();
10646         final ECEFVelocity ecefVelocity = new ECEFVelocity();
10647         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10648                 ecefPosition, ecefVelocity);
10649 
10650         final KnownPositionAccelerometerCalibrator calibrator =
10651                 new KnownPositionAccelerometerCalibrator(ecefPosition,
10652                         true, biasX, biasY, biasZ);
10653 
10654         // check default values
10655         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10656         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10657         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10658         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10659         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10660         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10661         final Acceleration bx2 = new Acceleration(0.0,
10662                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10663         calibrator.getInitialBiasXAsAcceleration(bx2);
10664         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10665         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10666         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10667         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10668         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10669         final Acceleration by2 = new Acceleration(0.0,
10670                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10671         calibrator.getInitialBiasYAsAcceleration(by2);
10672         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10673         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10674         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10675         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10676         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10677         final Acceleration bz2 = new Acceleration(0.0,
10678                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10679         calibrator.getInitialBiasZAsAcceleration(bz2);
10680         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10681         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10682         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10683         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10684         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10685         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10686         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10687         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10688         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10689         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10690         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10691         final double[] bias1 = calibrator.getInitialBias();
10692         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10693         final double[] bias2 = new double[3];
10694         calibrator.getInitialBias(bias2);
10695         assertArrayEquals(bias1, bias2, 0.0);
10696         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10697         assertEquals(b1, ba);
10698         final Matrix b2 = new Matrix(3, 1);
10699         calibrator.getInitialBiasAsMatrix(b2);
10700         assertEquals(b1, b2);
10701         final Matrix ma1 = calibrator.getInitialMa();
10702         assertEquals(ma1, new Matrix(3, 3));
10703         final Matrix ma2 = new Matrix(3, 3);
10704         calibrator.getInitialMa(ma2);
10705         assertEquals(ma1, ma2);
10706         assertSame(calibrator.getEcefPosition(), ecefPosition);
10707         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10708         final NEDPosition nedPosition1 = new NEDPosition();
10709         assertTrue(calibrator.getNedPosition(nedPosition1));
10710         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10711         assertNull(calibrator.getMeasurements());
10712         assertTrue(calibrator.isCommonAxisUsed());
10713         assertNull(calibrator.getListener());
10714         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
10715         assertFalse(calibrator.isReady());
10716         assertFalse(calibrator.isRunning());
10717         assertNull(calibrator.getEstimatedBiases());
10718         assertFalse(calibrator.getEstimatedBiases(null));
10719         assertNull(calibrator.getEstimatedBiasesAsMatrix());
10720         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10721         assertNull(calibrator.getEstimatedBiasFx());
10722         assertNull(calibrator.getEstimatedBiasFy());
10723         assertNull(calibrator.getEstimatedBiasFz());
10724         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10725         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10726         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10727         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10728         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10729         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10730         assertNull(calibrator.getEstimatedMa());
10731         assertNull(calibrator.getEstimatedSx());
10732         assertNull(calibrator.getEstimatedSy());
10733         assertNull(calibrator.getEstimatedSz());
10734         assertNull(calibrator.getEstimatedMxy());
10735         assertNull(calibrator.getEstimatedMxz());
10736         assertNull(calibrator.getEstimatedMyx());
10737         assertNull(calibrator.getEstimatedMyz());
10738         assertNull(calibrator.getEstimatedMzx());
10739         assertNull(calibrator.getEstimatedMzy());
10740         assertNull(calibrator.getEstimatedCovariance());
10741         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10742         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10743                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10744         assertNotNull(calibrator.getGroundTruthGravityNorm());
10745         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10746         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10747         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10748                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10749         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10750         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10751         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10752     }
10753 
10754     @Test
10755     public void testConstructor92() throws WrongSizeException {
10756         final Matrix ba = generateBa();
10757         final double biasX = ba.getElementAtIndex(0);
10758         final double biasY = ba.getElementAtIndex(1);
10759         final double biasZ = ba.getElementAtIndex(2);
10760 
10761         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10762         final double latitude = Math.toRadians(
10763                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10764         final double longitude = Math.toRadians(
10765                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10766         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10767         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10768         final NEDVelocity nedVelocity = new NEDVelocity();
10769         final ECEFPosition ecefPosition = new ECEFPosition();
10770         final ECEFVelocity ecefVelocity = new ECEFVelocity();
10771         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10772                 ecefPosition, ecefVelocity);
10773 
10774         final KnownPositionAccelerometerCalibrator calibrator =
10775                 new KnownPositionAccelerometerCalibrator(ecefPosition,
10776                         true, biasX, biasY, biasZ, this);
10777 
10778         // check default values
10779         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10780         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10781         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10782         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10783         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10784         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10785         final Acceleration bx2 = new Acceleration(0.0,
10786                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10787         calibrator.getInitialBiasXAsAcceleration(bx2);
10788         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10789         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10790         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10791         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10792         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10793         final Acceleration by2 = new Acceleration(0.0,
10794                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10795         calibrator.getInitialBiasYAsAcceleration(by2);
10796         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10797         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10798         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10799         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10800         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10801         final Acceleration bz2 = new Acceleration(0.0,
10802                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10803         calibrator.getInitialBiasZAsAcceleration(bz2);
10804         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10805         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10806         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10807         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10808         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10809         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10810         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10811         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10812         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10813         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10814         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10815         final double[] bias1 = calibrator.getInitialBias();
10816         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10817         final double[] bias2 = new double[3];
10818         calibrator.getInitialBias(bias2);
10819         assertArrayEquals(bias1, bias2, 0.0);
10820         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10821         assertEquals(b1, ba);
10822         final Matrix b2 = new Matrix(3, 1);
10823         calibrator.getInitialBiasAsMatrix(b2);
10824         assertEquals(b1, b2);
10825         final Matrix ma1 = calibrator.getInitialMa();
10826         assertEquals(ma1, new Matrix(3, 3));
10827         final Matrix ma2 = new Matrix(3, 3);
10828         calibrator.getInitialMa(ma2);
10829         assertEquals(ma1, ma2);
10830         assertSame(calibrator.getEcefPosition(), ecefPosition);
10831         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10832         final NEDPosition nedPosition1 = new NEDPosition();
10833         assertTrue(calibrator.getNedPosition(nedPosition1));
10834         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10835         assertNull(calibrator.getMeasurements());
10836         assertTrue(calibrator.isCommonAxisUsed());
10837         assertSame(calibrator.getListener(), this);
10838         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
10839         assertFalse(calibrator.isReady());
10840         assertFalse(calibrator.isRunning());
10841         assertNull(calibrator.getEstimatedBiases());
10842         assertFalse(calibrator.getEstimatedBiases(null));
10843         assertNull(calibrator.getEstimatedBiasesAsMatrix());
10844         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10845         assertNull(calibrator.getEstimatedBiasFx());
10846         assertNull(calibrator.getEstimatedBiasFy());
10847         assertNull(calibrator.getEstimatedBiasFz());
10848         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10849         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10850         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10851         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10852         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10853         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10854         assertNull(calibrator.getEstimatedMa());
10855         assertNull(calibrator.getEstimatedSx());
10856         assertNull(calibrator.getEstimatedSy());
10857         assertNull(calibrator.getEstimatedSz());
10858         assertNull(calibrator.getEstimatedMxy());
10859         assertNull(calibrator.getEstimatedMxz());
10860         assertNull(calibrator.getEstimatedMyx());
10861         assertNull(calibrator.getEstimatedMyz());
10862         assertNull(calibrator.getEstimatedMzx());
10863         assertNull(calibrator.getEstimatedMzy());
10864         assertNull(calibrator.getEstimatedCovariance());
10865         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10866         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10867                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10868         assertNotNull(calibrator.getGroundTruthGravityNorm());
10869         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10870         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10871         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10872                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10873         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10874         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10875         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10876     }
10877 
10878     @Test
10879     public void testConstructor93() throws WrongSizeException {
10880         final Collection<StandardDeviationBodyKinematics> measurements =
10881                 Collections.emptyList();
10882 
10883         final Matrix ba = generateBa();
10884         final double biasX = ba.getElementAtIndex(0);
10885         final double biasY = ba.getElementAtIndex(1);
10886         final double biasZ = ba.getElementAtIndex(2);
10887 
10888         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10889         final double latitude = Math.toRadians(
10890                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10891         final double longitude = Math.toRadians(
10892                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10893         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10894         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10895         final NEDVelocity nedVelocity = new NEDVelocity();
10896         final ECEFPosition ecefPosition = new ECEFPosition();
10897         final ECEFVelocity ecefVelocity = new ECEFVelocity();
10898         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10899                 ecefPosition, ecefVelocity);
10900 
10901         final KnownPositionAccelerometerCalibrator calibrator =
10902                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
10903                         true, biasX, biasY, biasZ);
10904 
10905         // check default values
10906         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10907         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10908         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10909         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10910         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10911         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10912         final Acceleration bx2 = new Acceleration(0.0,
10913                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10914         calibrator.getInitialBiasXAsAcceleration(bx2);
10915         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10916         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10917         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10918         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10919         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10920         final Acceleration by2 = new Acceleration(0.0,
10921                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10922         calibrator.getInitialBiasYAsAcceleration(by2);
10923         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10924         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10925         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10926         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10927         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10928         final Acceleration bz2 = new Acceleration(0.0,
10929                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10930         calibrator.getInitialBiasZAsAcceleration(bz2);
10931         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10932         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10933         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10934         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10935         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10936         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10937         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10938         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10939         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10940         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10941         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10942         final double[] bias1 = calibrator.getInitialBias();
10943         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10944         final double[] bias2 = new double[3];
10945         calibrator.getInitialBias(bias2);
10946         assertArrayEquals(bias1, bias2, 0.0);
10947         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10948         assertEquals(b1, ba);
10949         final Matrix b2 = new Matrix(3, 1);
10950         calibrator.getInitialBiasAsMatrix(b2);
10951         assertEquals(b1, b2);
10952         final Matrix ma1 = calibrator.getInitialMa();
10953         assertEquals(ma1, new Matrix(3, 3));
10954         final Matrix ma2 = new Matrix(3, 3);
10955         calibrator.getInitialMa(ma2);
10956         assertEquals(ma1, ma2);
10957         assertSame(calibrator.getEcefPosition(), ecefPosition);
10958         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10959         final NEDPosition nedPosition1 = new NEDPosition();
10960         assertTrue(calibrator.getNedPosition(nedPosition1));
10961         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10962         assertSame(calibrator.getMeasurements(), measurements);
10963         assertTrue(calibrator.isCommonAxisUsed());
10964         assertNull(calibrator.getListener());
10965         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
10966         assertFalse(calibrator.isReady());
10967         assertFalse(calibrator.isRunning());
10968         assertNull(calibrator.getEstimatedBiases());
10969         assertFalse(calibrator.getEstimatedBiases(null));
10970         assertNull(calibrator.getEstimatedBiasesAsMatrix());
10971         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10972         assertNull(calibrator.getEstimatedBiasFx());
10973         assertNull(calibrator.getEstimatedBiasFy());
10974         assertNull(calibrator.getEstimatedBiasFz());
10975         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10976         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10977         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10978         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10979         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10980         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10981         assertNull(calibrator.getEstimatedMa());
10982         assertNull(calibrator.getEstimatedSx());
10983         assertNull(calibrator.getEstimatedSy());
10984         assertNull(calibrator.getEstimatedSz());
10985         assertNull(calibrator.getEstimatedMxy());
10986         assertNull(calibrator.getEstimatedMxz());
10987         assertNull(calibrator.getEstimatedMyx());
10988         assertNull(calibrator.getEstimatedMyz());
10989         assertNull(calibrator.getEstimatedMzx());
10990         assertNull(calibrator.getEstimatedMzy());
10991         assertNull(calibrator.getEstimatedCovariance());
10992         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10993         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10994                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10995         assertNotNull(calibrator.getGroundTruthGravityNorm());
10996         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10997         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10998         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10999                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11000         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11001         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11002         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11003     }
11004 
11005     @Test
11006     public void testConstructor94() throws WrongSizeException {
11007         final Collection<StandardDeviationBodyKinematics> measurements =
11008                 Collections.emptyList();
11009 
11010         final Matrix ba = generateBa();
11011         final double biasX = ba.getElementAtIndex(0);
11012         final double biasY = ba.getElementAtIndex(1);
11013         final double biasZ = ba.getElementAtIndex(2);
11014 
11015         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11016         final double latitude = Math.toRadians(
11017                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11018         final double longitude = Math.toRadians(
11019                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11020         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11021         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11022         final NEDVelocity nedVelocity = new NEDVelocity();
11023         final ECEFPosition ecefPosition = new ECEFPosition();
11024         final ECEFVelocity ecefVelocity = new ECEFVelocity();
11025         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11026                 ecefPosition, ecefVelocity);
11027 
11028         final KnownPositionAccelerometerCalibrator calibrator =
11029                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
11030                         true, biasX, biasY, biasZ, this);
11031 
11032         // check default values
11033         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11034         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11035         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11036         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11037         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11038         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11039         final Acceleration bx2 = new Acceleration(0.0,
11040                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11041         calibrator.getInitialBiasXAsAcceleration(bx2);
11042         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11043         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11044         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11045         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11046         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11047         final Acceleration by2 = new Acceleration(0.0,
11048                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11049         calibrator.getInitialBiasYAsAcceleration(by2);
11050         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11051         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11052         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11053         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11054         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11055         final Acceleration bz2 = new Acceleration(0.0,
11056                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11057         calibrator.getInitialBiasZAsAcceleration(bz2);
11058         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11059         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11060         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11061         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11062         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11063         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11064         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11065         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11066         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11067         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11068         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11069         final double[] bias1 = calibrator.getInitialBias();
11070         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11071         final double[] bias2 = new double[3];
11072         calibrator.getInitialBias(bias2);
11073         assertArrayEquals(bias1, bias2, 0.0);
11074         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11075         assertEquals(b1, ba);
11076         final Matrix b2 = new Matrix(3, 1);
11077         calibrator.getInitialBiasAsMatrix(b2);
11078         assertEquals(b1, b2);
11079         final Matrix ma1 = calibrator.getInitialMa();
11080         assertEquals(ma1, new Matrix(3, 3));
11081         final Matrix ma2 = new Matrix(3, 3);
11082         calibrator.getInitialMa(ma2);
11083         assertEquals(ma1, ma2);
11084         assertSame(calibrator.getEcefPosition(), ecefPosition);
11085         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11086         final NEDPosition nedPosition1 = new NEDPosition();
11087         assertTrue(calibrator.getNedPosition(nedPosition1));
11088         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11089         assertSame(calibrator.getMeasurements(), measurements);
11090         assertTrue(calibrator.isCommonAxisUsed());
11091         assertSame(calibrator.getListener(), this);
11092         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
11093         assertFalse(calibrator.isReady());
11094         assertFalse(calibrator.isRunning());
11095         assertNull(calibrator.getEstimatedBiases());
11096         assertFalse(calibrator.getEstimatedBiases(null));
11097         assertNull(calibrator.getEstimatedBiasesAsMatrix());
11098         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11099         assertNull(calibrator.getEstimatedBiasFx());
11100         assertNull(calibrator.getEstimatedBiasFy());
11101         assertNull(calibrator.getEstimatedBiasFz());
11102         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11103         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11104         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11105         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11106         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11107         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11108         assertNull(calibrator.getEstimatedMa());
11109         assertNull(calibrator.getEstimatedSx());
11110         assertNull(calibrator.getEstimatedSy());
11111         assertNull(calibrator.getEstimatedSz());
11112         assertNull(calibrator.getEstimatedMxy());
11113         assertNull(calibrator.getEstimatedMxz());
11114         assertNull(calibrator.getEstimatedMyx());
11115         assertNull(calibrator.getEstimatedMyz());
11116         assertNull(calibrator.getEstimatedMzx());
11117         assertNull(calibrator.getEstimatedMzy());
11118         assertNull(calibrator.getEstimatedCovariance());
11119         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11120         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11121                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11122         assertNotNull(calibrator.getGroundTruthGravityNorm());
11123         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11124         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11125         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11126                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11127         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11128         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11129         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11130     }
11131 
11132     @Test
11133     public void testConstructor95() throws WrongSizeException {
11134         final Matrix ba = generateBa();
11135         final double biasX = ba.getElementAtIndex(0);
11136         final double biasY = ba.getElementAtIndex(1);
11137         final double biasZ = ba.getElementAtIndex(2);
11138 
11139         final Acceleration bx = new Acceleration(biasX,
11140                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11141         final Acceleration by = new Acceleration(biasY,
11142                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11143         final Acceleration bz = new Acceleration(biasZ,
11144                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11145 
11146         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11147         final double latitude = Math.toRadians(
11148                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11149         final double longitude = Math.toRadians(
11150                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11151         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11152         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11153         final NEDVelocity nedVelocity = new NEDVelocity();
11154         final ECEFPosition ecefPosition = new ECEFPosition();
11155         final ECEFVelocity ecefVelocity = new ECEFVelocity();
11156         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11157                 ecefPosition, ecefVelocity);
11158 
11159         final KnownPositionAccelerometerCalibrator calibrator =
11160                 new KnownPositionAccelerometerCalibrator(ecefPosition,
11161                         bx, by, bz);
11162 
11163         // check default values
11164         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11165         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11166         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11167         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11168         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11169         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11170         final Acceleration bx2 = new Acceleration(0.0,
11171                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11172         calibrator.getInitialBiasXAsAcceleration(bx2);
11173         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11174         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11175         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11176         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11177         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11178         final Acceleration by2 = new Acceleration(0.0,
11179                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11180         calibrator.getInitialBiasYAsAcceleration(by2);
11181         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11182         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11183         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11184         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11185         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11186         final Acceleration bz2 = new Acceleration(0.0,
11187                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11188         calibrator.getInitialBiasZAsAcceleration(bz2);
11189         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11190         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11191         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11192         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11193         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11194         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11195         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11196         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11197         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11198         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11199         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11200         final double[] bias1 = calibrator.getInitialBias();
11201         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11202         final double[] bias2 = new double[3];
11203         calibrator.getInitialBias(bias2);
11204         assertArrayEquals(bias1, bias2, 0.0);
11205         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11206         assertEquals(b1, ba);
11207         final Matrix b2 = new Matrix(3, 1);
11208         calibrator.getInitialBiasAsMatrix(b2);
11209         assertEquals(b1, b2);
11210         final Matrix ma1 = calibrator.getInitialMa();
11211         assertEquals(ma1, new Matrix(3, 3));
11212         final Matrix ma2 = new Matrix(3, 3);
11213         calibrator.getInitialMa(ma2);
11214         assertEquals(ma1, ma2);
11215         assertSame(calibrator.getEcefPosition(), ecefPosition);
11216         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11217         final NEDPosition nedPosition1 = new NEDPosition();
11218         assertTrue(calibrator.getNedPosition(nedPosition1));
11219         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11220         assertNull(calibrator.getMeasurements());
11221         assertFalse(calibrator.isCommonAxisUsed());
11222         assertNull(calibrator.getListener());
11223         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
11224         assertFalse(calibrator.isReady());
11225         assertFalse(calibrator.isRunning());
11226         assertNull(calibrator.getEstimatedBiases());
11227         assertFalse(calibrator.getEstimatedBiases(null));
11228         assertNull(calibrator.getEstimatedBiasesAsMatrix());
11229         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11230         assertNull(calibrator.getEstimatedBiasFx());
11231         assertNull(calibrator.getEstimatedBiasFy());
11232         assertNull(calibrator.getEstimatedBiasFz());
11233         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11234         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11235         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11236         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11237         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11238         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11239         assertNull(calibrator.getEstimatedMa());
11240         assertNull(calibrator.getEstimatedSx());
11241         assertNull(calibrator.getEstimatedSy());
11242         assertNull(calibrator.getEstimatedSz());
11243         assertNull(calibrator.getEstimatedMxy());
11244         assertNull(calibrator.getEstimatedMxz());
11245         assertNull(calibrator.getEstimatedMyx());
11246         assertNull(calibrator.getEstimatedMyz());
11247         assertNull(calibrator.getEstimatedMzx());
11248         assertNull(calibrator.getEstimatedMzy());
11249         assertNull(calibrator.getEstimatedCovariance());
11250         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11251         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11252                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11253         assertNotNull(calibrator.getGroundTruthGravityNorm());
11254         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11255         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11256         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11257                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11258         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11259         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11260         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11261     }
11262 
11263     @Test
11264     public void testConstructor96() throws WrongSizeException {
11265         final Matrix ba = generateBa();
11266         final double biasX = ba.getElementAtIndex(0);
11267         final double biasY = ba.getElementAtIndex(1);
11268         final double biasZ = ba.getElementAtIndex(2);
11269 
11270         final Acceleration bx = new Acceleration(biasX,
11271                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11272         final Acceleration by = new Acceleration(biasY,
11273                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11274         final Acceleration bz = new Acceleration(biasZ,
11275                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11276 
11277         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11278         final double latitude = Math.toRadians(
11279                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11280         final double longitude = Math.toRadians(
11281                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11282         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11283         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11284         final NEDVelocity nedVelocity = new NEDVelocity();
11285         final ECEFPosition ecefPosition = new ECEFPosition();
11286         final ECEFVelocity ecefVelocity = new ECEFVelocity();
11287         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11288                 ecefPosition, ecefVelocity);
11289 
11290         final KnownPositionAccelerometerCalibrator calibrator =
11291                 new KnownPositionAccelerometerCalibrator(ecefPosition,
11292                         bx, by, bz, this);
11293 
11294         // check default values
11295         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11296         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11297         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11298         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11299         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11300         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11301         final Acceleration bx2 = new Acceleration(0.0,
11302                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11303         calibrator.getInitialBiasXAsAcceleration(bx2);
11304         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11305         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11306         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11307         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11308         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11309         final Acceleration by2 = new Acceleration(0.0,
11310                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11311         calibrator.getInitialBiasYAsAcceleration(by2);
11312         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11313         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11314         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11315         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11316         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11317         final Acceleration bz2 = new Acceleration(0.0,
11318                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11319         calibrator.getInitialBiasZAsAcceleration(bz2);
11320         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11321         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11322         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11323         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11324         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11325         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11326         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11327         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11328         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11329         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11330         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11331         final double[] bias1 = calibrator.getInitialBias();
11332         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11333         final double[] bias2 = new double[3];
11334         calibrator.getInitialBias(bias2);
11335         assertArrayEquals(bias1, bias2, 0.0);
11336         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11337         assertEquals(b1, ba);
11338         final Matrix b2 = new Matrix(3, 1);
11339         calibrator.getInitialBiasAsMatrix(b2);
11340         assertEquals(b1, b2);
11341         final Matrix ma1 = calibrator.getInitialMa();
11342         assertEquals(ma1, new Matrix(3, 3));
11343         final Matrix ma2 = new Matrix(3, 3);
11344         calibrator.getInitialMa(ma2);
11345         assertEquals(ma1, ma2);
11346         assertSame(calibrator.getEcefPosition(), ecefPosition);
11347         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11348         final NEDPosition nedPosition1 = new NEDPosition();
11349         assertTrue(calibrator.getNedPosition(nedPosition1));
11350         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11351         assertNull(calibrator.getMeasurements());
11352         assertFalse(calibrator.isCommonAxisUsed());
11353         assertSame(calibrator.getListener(), this);
11354         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
11355         assertFalse(calibrator.isReady());
11356         assertFalse(calibrator.isRunning());
11357         assertNull(calibrator.getEstimatedBiases());
11358         assertFalse(calibrator.getEstimatedBiases(null));
11359         assertNull(calibrator.getEstimatedBiasesAsMatrix());
11360         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11361         assertNull(calibrator.getEstimatedBiasFx());
11362         assertNull(calibrator.getEstimatedBiasFy());
11363         assertNull(calibrator.getEstimatedBiasFz());
11364         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11365         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11366         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11367         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11368         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11369         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11370         assertNull(calibrator.getEstimatedMa());
11371         assertNull(calibrator.getEstimatedSx());
11372         assertNull(calibrator.getEstimatedSy());
11373         assertNull(calibrator.getEstimatedSz());
11374         assertNull(calibrator.getEstimatedMxy());
11375         assertNull(calibrator.getEstimatedMxz());
11376         assertNull(calibrator.getEstimatedMyx());
11377         assertNull(calibrator.getEstimatedMyz());
11378         assertNull(calibrator.getEstimatedMzx());
11379         assertNull(calibrator.getEstimatedMzy());
11380         assertNull(calibrator.getEstimatedCovariance());
11381         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11382         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11383                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11384         assertNotNull(calibrator.getGroundTruthGravityNorm());
11385         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11386         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11387         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11388                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11389         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11390         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11391         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11392     }
11393 
11394     @Test
11395     public void testConstructor97() throws WrongSizeException {
11396         final Collection<StandardDeviationBodyKinematics> measurements =
11397                 Collections.emptyList();
11398 
11399         final Matrix ba = generateBa();
11400         final double biasX = ba.getElementAtIndex(0);
11401         final double biasY = ba.getElementAtIndex(1);
11402         final double biasZ = ba.getElementAtIndex(2);
11403 
11404         final Acceleration bx = new Acceleration(biasX,
11405                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11406         final Acceleration by = new Acceleration(biasY,
11407                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11408         final Acceleration bz = new Acceleration(biasZ,
11409                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11410 
11411         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11412         final double latitude = Math.toRadians(
11413                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11414         final double longitude = Math.toRadians(
11415                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11416         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11417         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11418         final NEDVelocity nedVelocity = new NEDVelocity();
11419         final ECEFPosition ecefPosition = new ECEFPosition();
11420         final ECEFVelocity ecefVelocity = new ECEFVelocity();
11421         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11422                 ecefPosition, ecefVelocity);
11423 
11424         final KnownPositionAccelerometerCalibrator calibrator =
11425                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
11426                         bx, by, bz);
11427 
11428         // check default values
11429         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11430         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11431         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11432         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11433         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11434         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11435         final Acceleration bx2 = new Acceleration(0.0,
11436                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11437         calibrator.getInitialBiasXAsAcceleration(bx2);
11438         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11439         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11440         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11441         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11442         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11443         final Acceleration by2 = new Acceleration(0.0,
11444                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11445         calibrator.getInitialBiasYAsAcceleration(by2);
11446         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11447         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11448         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11449         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11450         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11451         final Acceleration bz2 = new Acceleration(0.0,
11452                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11453         calibrator.getInitialBiasZAsAcceleration(bz2);
11454         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11455         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11456         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11457         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11458         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11459         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11460         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11461         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11462         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11463         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11464         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11465         final double[] bias1 = calibrator.getInitialBias();
11466         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11467         final double[] bias2 = new double[3];
11468         calibrator.getInitialBias(bias2);
11469         assertArrayEquals(bias1, bias2, 0.0);
11470         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11471         assertEquals(b1, ba);
11472         final Matrix b2 = new Matrix(3, 1);
11473         calibrator.getInitialBiasAsMatrix(b2);
11474         assertEquals(b1, b2);
11475         final Matrix ma1 = calibrator.getInitialMa();
11476         assertEquals(ma1, new Matrix(3, 3));
11477         final Matrix ma2 = new Matrix(3, 3);
11478         calibrator.getInitialMa(ma2);
11479         assertEquals(ma1, ma2);
11480         assertSame(calibrator.getEcefPosition(), ecefPosition);
11481         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11482         final NEDPosition nedPosition1 = new NEDPosition();
11483         assertTrue(calibrator.getNedPosition(nedPosition1));
11484         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11485         assertSame(calibrator.getMeasurements(), measurements);
11486         assertFalse(calibrator.isCommonAxisUsed());
11487         assertNull(calibrator.getListener());
11488         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
11489         assertFalse(calibrator.isReady());
11490         assertFalse(calibrator.isRunning());
11491         assertNull(calibrator.getEstimatedBiases());
11492         assertFalse(calibrator.getEstimatedBiases(null));
11493         assertNull(calibrator.getEstimatedBiasesAsMatrix());
11494         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11495         assertNull(calibrator.getEstimatedBiasFx());
11496         assertNull(calibrator.getEstimatedBiasFy());
11497         assertNull(calibrator.getEstimatedBiasFz());
11498         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11499         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11500         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11501         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11502         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11503         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11504         assertNull(calibrator.getEstimatedMa());
11505         assertNull(calibrator.getEstimatedSx());
11506         assertNull(calibrator.getEstimatedSy());
11507         assertNull(calibrator.getEstimatedSz());
11508         assertNull(calibrator.getEstimatedMxy());
11509         assertNull(calibrator.getEstimatedMxz());
11510         assertNull(calibrator.getEstimatedMyx());
11511         assertNull(calibrator.getEstimatedMyz());
11512         assertNull(calibrator.getEstimatedMzx());
11513         assertNull(calibrator.getEstimatedMzy());
11514         assertNull(calibrator.getEstimatedCovariance());
11515         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11516         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11517                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11518         assertNotNull(calibrator.getGroundTruthGravityNorm());
11519         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11520         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11521         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11522                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11523         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11524         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11525         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11526     }
11527 
11528     @Test
11529     public void testConstructor98() throws WrongSizeException {
11530         final Collection<StandardDeviationBodyKinematics> measurements =
11531                 Collections.emptyList();
11532 
11533         final Matrix ba = generateBa();
11534         final double biasX = ba.getElementAtIndex(0);
11535         final double biasY = ba.getElementAtIndex(1);
11536         final double biasZ = ba.getElementAtIndex(2);
11537 
11538         final Acceleration bx = new Acceleration(biasX,
11539                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11540         final Acceleration by = new Acceleration(biasY,
11541                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11542         final Acceleration bz = new Acceleration(biasZ,
11543                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11544 
11545         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11546         final double latitude = Math.toRadians(
11547                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11548         final double longitude = Math.toRadians(
11549                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11550         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11551         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11552         final NEDVelocity nedVelocity = new NEDVelocity();
11553         final ECEFPosition ecefPosition = new ECEFPosition();
11554         final ECEFVelocity ecefVelocity = new ECEFVelocity();
11555         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11556                 ecefPosition, ecefVelocity);
11557 
11558         final KnownPositionAccelerometerCalibrator calibrator =
11559                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
11560                         bx, by, bz, this);
11561 
11562         // check default values
11563         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11564         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11565         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11566         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11567         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11568         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11569         final Acceleration bx2 = new Acceleration(0.0,
11570                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11571         calibrator.getInitialBiasXAsAcceleration(bx2);
11572         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11573         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11574         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11575         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11576         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11577         final Acceleration by2 = new Acceleration(0.0,
11578                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11579         calibrator.getInitialBiasYAsAcceleration(by2);
11580         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11581         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11582         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11583         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11584         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11585         final Acceleration bz2 = new Acceleration(0.0,
11586                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11587         calibrator.getInitialBiasZAsAcceleration(bz2);
11588         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11589         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11590         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11591         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11592         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11593         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11594         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11595         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11596         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11597         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11598         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11599         final double[] bias1 = calibrator.getInitialBias();
11600         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11601         final double[] bias2 = new double[3];
11602         calibrator.getInitialBias(bias2);
11603         assertArrayEquals(bias1, bias2, 0.0);
11604         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11605         assertEquals(b1, ba);
11606         final Matrix b2 = new Matrix(3, 1);
11607         calibrator.getInitialBiasAsMatrix(b2);
11608         assertEquals(b1, b2);
11609         final Matrix ma1 = calibrator.getInitialMa();
11610         assertEquals(ma1, new Matrix(3, 3));
11611         final Matrix ma2 = new Matrix(3, 3);
11612         calibrator.getInitialMa(ma2);
11613         assertEquals(ma1, ma2);
11614         assertSame(calibrator.getEcefPosition(), ecefPosition);
11615         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11616         final NEDPosition nedPosition1 = new NEDPosition();
11617         assertTrue(calibrator.getNedPosition(nedPosition1));
11618         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11619         assertSame(calibrator.getMeasurements(), measurements);
11620         assertFalse(calibrator.isCommonAxisUsed());
11621         assertSame(calibrator.getListener(), this);
11622         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
11623         assertFalse(calibrator.isReady());
11624         assertFalse(calibrator.isRunning());
11625         assertNull(calibrator.getEstimatedBiases());
11626         assertFalse(calibrator.getEstimatedBiases(null));
11627         assertNull(calibrator.getEstimatedBiasesAsMatrix());
11628         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11629         assertNull(calibrator.getEstimatedBiasFx());
11630         assertNull(calibrator.getEstimatedBiasFy());
11631         assertNull(calibrator.getEstimatedBiasFz());
11632         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11633         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11634         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11635         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11636         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11637         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11638         assertNull(calibrator.getEstimatedMa());
11639         assertNull(calibrator.getEstimatedSx());
11640         assertNull(calibrator.getEstimatedSy());
11641         assertNull(calibrator.getEstimatedSz());
11642         assertNull(calibrator.getEstimatedMxy());
11643         assertNull(calibrator.getEstimatedMxz());
11644         assertNull(calibrator.getEstimatedMyx());
11645         assertNull(calibrator.getEstimatedMyz());
11646         assertNull(calibrator.getEstimatedMzx());
11647         assertNull(calibrator.getEstimatedMzy());
11648         assertNull(calibrator.getEstimatedCovariance());
11649         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11650         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11651                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11652         assertNotNull(calibrator.getGroundTruthGravityNorm());
11653         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11654         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11655         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11656                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11657         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11658         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11659         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11660     }
11661 
11662     @Test
11663     public void testConstructor99() throws WrongSizeException {
11664         final Matrix ba = generateBa();
11665         final double biasX = ba.getElementAtIndex(0);
11666         final double biasY = ba.getElementAtIndex(1);
11667         final double biasZ = ba.getElementAtIndex(2);
11668 
11669         final Acceleration bx = new Acceleration(biasX,
11670                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11671         final Acceleration by = new Acceleration(biasY,
11672                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11673         final Acceleration bz = new Acceleration(biasZ,
11674                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11675 
11676         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11677         final double latitude = Math.toRadians(
11678                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11679         final double longitude = Math.toRadians(
11680                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11681         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11682         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11683         final NEDVelocity nedVelocity = new NEDVelocity();
11684         final ECEFPosition ecefPosition = new ECEFPosition();
11685         final ECEFVelocity ecefVelocity = new ECEFVelocity();
11686         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11687                 ecefPosition, ecefVelocity);
11688 
11689         final KnownPositionAccelerometerCalibrator calibrator =
11690                 new KnownPositionAccelerometerCalibrator(ecefPosition,
11691                         true, bx, by, bz);
11692 
11693         // check default values
11694         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11695         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11696         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11697         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11698         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11699         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11700         final Acceleration bx2 = new Acceleration(0.0,
11701                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11702         calibrator.getInitialBiasXAsAcceleration(bx2);
11703         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11704         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11705         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11706         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11707         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11708         final Acceleration by2 = new Acceleration(0.0,
11709                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11710         calibrator.getInitialBiasYAsAcceleration(by2);
11711         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11712         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11713         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11714         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11715         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11716         final Acceleration bz2 = new Acceleration(0.0,
11717                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11718         calibrator.getInitialBiasZAsAcceleration(bz2);
11719         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11720         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11721         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11722         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11723         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11724         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11725         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11726         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11727         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11728         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11729         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11730         final double[] bias1 = calibrator.getInitialBias();
11731         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11732         final double[] bias2 = new double[3];
11733         calibrator.getInitialBias(bias2);
11734         assertArrayEquals(bias1, bias2, 0.0);
11735         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11736         assertEquals(b1, ba);
11737         final Matrix b2 = new Matrix(3, 1);
11738         calibrator.getInitialBiasAsMatrix(b2);
11739         assertEquals(b1, b2);
11740         final Matrix ma1 = calibrator.getInitialMa();
11741         assertEquals(ma1, new Matrix(3, 3));
11742         final Matrix ma2 = new Matrix(3, 3);
11743         calibrator.getInitialMa(ma2);
11744         assertEquals(ma1, ma2);
11745         assertSame(calibrator.getEcefPosition(), ecefPosition);
11746         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11747         final NEDPosition nedPosition1 = new NEDPosition();
11748         assertTrue(calibrator.getNedPosition(nedPosition1));
11749         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11750         assertNull(calibrator.getMeasurements());
11751         assertTrue(calibrator.isCommonAxisUsed());
11752         assertNull(calibrator.getListener());
11753         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
11754         assertFalse(calibrator.isReady());
11755         assertFalse(calibrator.isRunning());
11756         assertNull(calibrator.getEstimatedBiases());
11757         assertFalse(calibrator.getEstimatedBiases(null));
11758         assertNull(calibrator.getEstimatedBiasesAsMatrix());
11759         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11760         assertNull(calibrator.getEstimatedBiasFx());
11761         assertNull(calibrator.getEstimatedBiasFy());
11762         assertNull(calibrator.getEstimatedBiasFz());
11763         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11764         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11765         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11766         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11767         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11768         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11769         assertNull(calibrator.getEstimatedMa());
11770         assertNull(calibrator.getEstimatedSx());
11771         assertNull(calibrator.getEstimatedSy());
11772         assertNull(calibrator.getEstimatedSz());
11773         assertNull(calibrator.getEstimatedMxy());
11774         assertNull(calibrator.getEstimatedMxz());
11775         assertNull(calibrator.getEstimatedMyx());
11776         assertNull(calibrator.getEstimatedMyz());
11777         assertNull(calibrator.getEstimatedMzx());
11778         assertNull(calibrator.getEstimatedMzy());
11779         assertNull(calibrator.getEstimatedCovariance());
11780         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11781         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11782                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11783         assertNotNull(calibrator.getGroundTruthGravityNorm());
11784         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11785         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11786         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11787                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11788         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11789         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11790         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11791     }
11792 
11793     @Test
11794     public void testConstructor100() throws WrongSizeException {
11795         final Matrix ba = generateBa();
11796         final double biasX = ba.getElementAtIndex(0);
11797         final double biasY = ba.getElementAtIndex(1);
11798         final double biasZ = ba.getElementAtIndex(2);
11799 
11800         final Acceleration bx = new Acceleration(biasX,
11801                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11802         final Acceleration by = new Acceleration(biasY,
11803                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11804         final Acceleration bz = new Acceleration(biasZ,
11805                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11806 
11807         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11808         final double latitude = Math.toRadians(
11809                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11810         final double longitude = Math.toRadians(
11811                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11812         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11813         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11814         final NEDVelocity nedVelocity = new NEDVelocity();
11815         final ECEFPosition ecefPosition = new ECEFPosition();
11816         final ECEFVelocity ecefVelocity = new ECEFVelocity();
11817         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11818                 ecefPosition, ecefVelocity);
11819 
11820         final KnownPositionAccelerometerCalibrator calibrator =
11821                 new KnownPositionAccelerometerCalibrator(ecefPosition,
11822                         true, bx, by, bz, this);
11823 
11824         // check default values
11825         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11826         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11827         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11828         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11829         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11830         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11831         final Acceleration bx2 = new Acceleration(0.0,
11832                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11833         calibrator.getInitialBiasXAsAcceleration(bx2);
11834         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11835         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11836         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11837         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11838         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11839         final Acceleration by2 = new Acceleration(0.0,
11840                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11841         calibrator.getInitialBiasYAsAcceleration(by2);
11842         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11843         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11844         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11845         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11846         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11847         final Acceleration bz2 = new Acceleration(0.0,
11848                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11849         calibrator.getInitialBiasZAsAcceleration(bz2);
11850         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11851         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11852         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11853         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11854         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11855         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11856         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11857         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11858         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11859         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11860         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11861         final double[] bias1 = calibrator.getInitialBias();
11862         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11863         final double[] bias2 = new double[3];
11864         calibrator.getInitialBias(bias2);
11865         assertArrayEquals(bias1, bias2, 0.0);
11866         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11867         assertEquals(b1, ba);
11868         final Matrix b2 = new Matrix(3, 1);
11869         calibrator.getInitialBiasAsMatrix(b2);
11870         assertEquals(b1, b2);
11871         final Matrix ma1 = calibrator.getInitialMa();
11872         assertEquals(ma1, new Matrix(3, 3));
11873         final Matrix ma2 = new Matrix(3, 3);
11874         calibrator.getInitialMa(ma2);
11875         assertEquals(ma1, ma2);
11876         assertSame(calibrator.getEcefPosition(), ecefPosition);
11877         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11878         final NEDPosition nedPosition1 = new NEDPosition();
11879         assertTrue(calibrator.getNedPosition(nedPosition1));
11880         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11881         assertNull(calibrator.getMeasurements());
11882         assertTrue(calibrator.isCommonAxisUsed());
11883         assertSame(calibrator.getListener(), this);
11884         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
11885         assertFalse(calibrator.isReady());
11886         assertFalse(calibrator.isRunning());
11887         assertNull(calibrator.getEstimatedBiases());
11888         assertFalse(calibrator.getEstimatedBiases(null));
11889         assertNull(calibrator.getEstimatedBiasesAsMatrix());
11890         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11891         assertNull(calibrator.getEstimatedBiasFx());
11892         assertNull(calibrator.getEstimatedBiasFy());
11893         assertNull(calibrator.getEstimatedBiasFz());
11894         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11895         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11896         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11897         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11898         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11899         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11900         assertNull(calibrator.getEstimatedMa());
11901         assertNull(calibrator.getEstimatedSx());
11902         assertNull(calibrator.getEstimatedSy());
11903         assertNull(calibrator.getEstimatedSz());
11904         assertNull(calibrator.getEstimatedMxy());
11905         assertNull(calibrator.getEstimatedMxz());
11906         assertNull(calibrator.getEstimatedMyx());
11907         assertNull(calibrator.getEstimatedMyz());
11908         assertNull(calibrator.getEstimatedMzx());
11909         assertNull(calibrator.getEstimatedMzy());
11910         assertNull(calibrator.getEstimatedCovariance());
11911         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11912         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11913                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11914         assertNotNull(calibrator.getGroundTruthGravityNorm());
11915         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11916         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11917         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11918                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11919         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11920         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11921         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11922     }
11923 
11924     @Test
11925     public void testConstructor101() throws WrongSizeException {
11926         final Collection<StandardDeviationBodyKinematics> measurements =
11927                 Collections.emptyList();
11928 
11929         final Matrix ba = generateBa();
11930         final double biasX = ba.getElementAtIndex(0);
11931         final double biasY = ba.getElementAtIndex(1);
11932         final double biasZ = ba.getElementAtIndex(2);
11933 
11934         final Acceleration bx = new Acceleration(biasX,
11935                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11936         final Acceleration by = new Acceleration(biasY,
11937                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11938         final Acceleration bz = new Acceleration(biasZ,
11939                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11940 
11941         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11942         final double latitude = Math.toRadians(
11943                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11944         final double longitude = Math.toRadians(
11945                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11946         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11947         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11948         final NEDVelocity nedVelocity = new NEDVelocity();
11949         final ECEFPosition ecefPosition = new ECEFPosition();
11950         final ECEFVelocity ecefVelocity = new ECEFVelocity();
11951         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11952                 ecefPosition, ecefVelocity);
11953 
11954         final KnownPositionAccelerometerCalibrator calibrator =
11955                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
11956                         true, bx, by, bz);
11957 
11958         // check default values
11959         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11960         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11961         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11962         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11963         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11964         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11965         final Acceleration bx2 = new Acceleration(0.0,
11966                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11967         calibrator.getInitialBiasXAsAcceleration(bx2);
11968         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11969         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11970         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11971         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11972         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11973         final Acceleration by2 = new Acceleration(0.0,
11974                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11975         calibrator.getInitialBiasYAsAcceleration(by2);
11976         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11977         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11978         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11979         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11980         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11981         final Acceleration bz2 = new Acceleration(0.0,
11982                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11983         calibrator.getInitialBiasZAsAcceleration(bz2);
11984         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11985         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11986         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11987         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11988         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11989         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11990         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11991         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11992         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11993         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11994         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11995         final double[] bias1 = calibrator.getInitialBias();
11996         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11997         final double[] bias2 = new double[3];
11998         calibrator.getInitialBias(bias2);
11999         assertArrayEquals(bias1, bias2, 0.0);
12000         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12001         assertEquals(b1, ba);
12002         final Matrix b2 = new Matrix(3, 1);
12003         calibrator.getInitialBiasAsMatrix(b2);
12004         assertEquals(b1, b2);
12005         final Matrix ma1 = calibrator.getInitialMa();
12006         assertEquals(ma1, new Matrix(3, 3));
12007         final Matrix ma2 = new Matrix(3, 3);
12008         calibrator.getInitialMa(ma2);
12009         assertEquals(ma1, ma2);
12010         assertSame(calibrator.getEcefPosition(), ecefPosition);
12011         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12012         final NEDPosition nedPosition1 = new NEDPosition();
12013         assertTrue(calibrator.getNedPosition(nedPosition1));
12014         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12015         assertSame(calibrator.getMeasurements(), measurements);
12016         assertTrue(calibrator.isCommonAxisUsed());
12017         assertNull(calibrator.getListener());
12018         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12019         assertFalse(calibrator.isReady());
12020         assertFalse(calibrator.isRunning());
12021         assertNull(calibrator.getEstimatedBiases());
12022         assertFalse(calibrator.getEstimatedBiases(null));
12023         assertNull(calibrator.getEstimatedBiasesAsMatrix());
12024         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12025         assertNull(calibrator.getEstimatedBiasFx());
12026         assertNull(calibrator.getEstimatedBiasFy());
12027         assertNull(calibrator.getEstimatedBiasFz());
12028         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12029         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12030         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12031         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12032         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12033         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12034         assertNull(calibrator.getEstimatedMa());
12035         assertNull(calibrator.getEstimatedSx());
12036         assertNull(calibrator.getEstimatedSy());
12037         assertNull(calibrator.getEstimatedSz());
12038         assertNull(calibrator.getEstimatedMxy());
12039         assertNull(calibrator.getEstimatedMxz());
12040         assertNull(calibrator.getEstimatedMyx());
12041         assertNull(calibrator.getEstimatedMyz());
12042         assertNull(calibrator.getEstimatedMzx());
12043         assertNull(calibrator.getEstimatedMzy());
12044         assertNull(calibrator.getEstimatedCovariance());
12045         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12046         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12047                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12048         assertNotNull(calibrator.getGroundTruthGravityNorm());
12049         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12050         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12051         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12052                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12053         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12054         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12055         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12056     }
12057 
12058     @Test
12059     public void testConstructor102() throws WrongSizeException {
12060         final Collection<StandardDeviationBodyKinematics> measurements =
12061                 Collections.emptyList();
12062 
12063         final Matrix ba = generateBa();
12064         final double biasX = ba.getElementAtIndex(0);
12065         final double biasY = ba.getElementAtIndex(1);
12066         final double biasZ = ba.getElementAtIndex(2);
12067 
12068         final Acceleration bx = new Acceleration(biasX,
12069                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12070         final Acceleration by = new Acceleration(biasY,
12071                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12072         final Acceleration bz = new Acceleration(biasZ,
12073                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12074 
12075         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12076         final double latitude = Math.toRadians(
12077                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12078         final double longitude = Math.toRadians(
12079                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12080         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12081         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12082         final NEDVelocity nedVelocity = new NEDVelocity();
12083         final ECEFPosition ecefPosition = new ECEFPosition();
12084         final ECEFVelocity ecefVelocity = new ECEFVelocity();
12085         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12086                 ecefPosition, ecefVelocity);
12087 
12088         final KnownPositionAccelerometerCalibrator calibrator =
12089                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
12090                         true, bx, by, bz, this);
12091 
12092         // check default values
12093         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12094         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12095         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12096         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12097         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12098         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12099         final Acceleration bx2 = new Acceleration(0.0,
12100                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12101         calibrator.getInitialBiasXAsAcceleration(bx2);
12102         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12103         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12104         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12105         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12106         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12107         final Acceleration by2 = new Acceleration(0.0,
12108                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12109         calibrator.getInitialBiasYAsAcceleration(by2);
12110         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12111         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12112         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12113         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12114         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12115         final Acceleration bz2 = new Acceleration(0.0,
12116                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12117         calibrator.getInitialBiasZAsAcceleration(bz2);
12118         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12119         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12120         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
12121         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
12122         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
12123         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12124         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12125         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12126         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12127         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12128         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12129         final double[] bias1 = calibrator.getInitialBias();
12130         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12131         final double[] bias2 = new double[3];
12132         calibrator.getInitialBias(bias2);
12133         assertArrayEquals(bias1, bias2, 0.0);
12134         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12135         assertEquals(b1, ba);
12136         final Matrix b2 = new Matrix(3, 1);
12137         calibrator.getInitialBiasAsMatrix(b2);
12138         assertEquals(b1, b2);
12139         final Matrix ma1 = calibrator.getInitialMa();
12140         assertEquals(ma1, new Matrix(3, 3));
12141         final Matrix ma2 = new Matrix(3, 3);
12142         calibrator.getInitialMa(ma2);
12143         assertEquals(ma1, ma2);
12144         assertSame(calibrator.getEcefPosition(), ecefPosition);
12145         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12146         final NEDPosition nedPosition1 = new NEDPosition();
12147         assertTrue(calibrator.getNedPosition(nedPosition1));
12148         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12149         assertSame(calibrator.getMeasurements(), measurements);
12150         assertTrue(calibrator.isCommonAxisUsed());
12151         assertSame(calibrator.getListener(), this);
12152         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12153         assertFalse(calibrator.isReady());
12154         assertFalse(calibrator.isRunning());
12155         assertNull(calibrator.getEstimatedBiases());
12156         assertFalse(calibrator.getEstimatedBiases(null));
12157         assertNull(calibrator.getEstimatedBiasesAsMatrix());
12158         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12159         assertNull(calibrator.getEstimatedBiasFx());
12160         assertNull(calibrator.getEstimatedBiasFy());
12161         assertNull(calibrator.getEstimatedBiasFz());
12162         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12163         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12164         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12165         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12166         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12167         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12168         assertNull(calibrator.getEstimatedMa());
12169         assertNull(calibrator.getEstimatedSx());
12170         assertNull(calibrator.getEstimatedSy());
12171         assertNull(calibrator.getEstimatedSz());
12172         assertNull(calibrator.getEstimatedMxy());
12173         assertNull(calibrator.getEstimatedMxz());
12174         assertNull(calibrator.getEstimatedMyx());
12175         assertNull(calibrator.getEstimatedMyz());
12176         assertNull(calibrator.getEstimatedMzx());
12177         assertNull(calibrator.getEstimatedMzy());
12178         assertNull(calibrator.getEstimatedCovariance());
12179         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12180         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12181                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12182         assertNotNull(calibrator.getGroundTruthGravityNorm());
12183         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12184         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12185         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12186                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12187         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12188         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12189         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12190     }
12191 
12192     @Test
12193     public void testConstructor103() throws WrongSizeException {
12194         final Matrix ba = generateBa();
12195         final double biasX = ba.getElementAtIndex(0);
12196         final double biasY = ba.getElementAtIndex(1);
12197         final double biasZ = ba.getElementAtIndex(2);
12198 
12199         final Matrix ma = generateMaCommonAxis();
12200         final double sx = ma.getElementAt(0, 0);
12201         final double sy = ma.getElementAt(1, 1);
12202         final double sz = ma.getElementAt(2, 2);
12203 
12204         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12205         final double latitude = Math.toRadians(
12206                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12207         final double longitude = Math.toRadians(
12208                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12209         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12210         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12211         final NEDVelocity nedVelocity = new NEDVelocity();
12212         final ECEFPosition ecefPosition = new ECEFPosition();
12213         final ECEFVelocity ecefVelocity = new ECEFVelocity();
12214         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12215                 ecefPosition, ecefVelocity);
12216 
12217         final KnownPositionAccelerometerCalibrator calibrator =
12218                 new KnownPositionAccelerometerCalibrator(ecefPosition,
12219                         biasX, biasY, biasZ, sx, sy, sz);
12220 
12221         // check default values
12222         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12223         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12224         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12225         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12226         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12227         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12228         final Acceleration bx2 = new Acceleration(0.0,
12229                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12230         calibrator.getInitialBiasXAsAcceleration(bx2);
12231         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12232         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12233         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12234         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12235         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12236         final Acceleration by2 = new Acceleration(0.0,
12237                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12238         calibrator.getInitialBiasYAsAcceleration(by2);
12239         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12240         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12241         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12242         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12243         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12244         final Acceleration bz2 = new Acceleration(0.0,
12245                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12246         calibrator.getInitialBiasZAsAcceleration(bz2);
12247         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12248         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12249         assertEquals(calibrator.getInitialSx(), sx, 0.0);
12250         assertEquals(calibrator.getInitialSy(), sy, 0.0);
12251         assertEquals(calibrator.getInitialSz(), sz, 0.0);
12252         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12253         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12254         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12255         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12256         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12257         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12258         final double[] bias1 = calibrator.getInitialBias();
12259         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12260         final double[] bias2 = new double[3];
12261         calibrator.getInitialBias(bias2);
12262         assertArrayEquals(bias1, bias2, 0.0);
12263         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12264         assertEquals(b1, ba);
12265         final Matrix b2 = new Matrix(3, 1);
12266         calibrator.getInitialBiasAsMatrix(b2);
12267         assertEquals(b1, b2);
12268         final Matrix ma1 = calibrator.getInitialMa();
12269         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12270         final Matrix ma2 = new Matrix(3, 3);
12271         calibrator.getInitialMa(ma2);
12272         assertEquals(ma1, ma2);
12273         assertSame(calibrator.getEcefPosition(), ecefPosition);
12274         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12275         final NEDPosition nedPosition1 = new NEDPosition();
12276         assertTrue(calibrator.getNedPosition(nedPosition1));
12277         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12278         assertNull(calibrator.getMeasurements());
12279         assertFalse(calibrator.isCommonAxisUsed());
12280         assertNull(calibrator.getListener());
12281         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
12282         assertFalse(calibrator.isReady());
12283         assertFalse(calibrator.isRunning());
12284         assertNull(calibrator.getEstimatedBiases());
12285         assertFalse(calibrator.getEstimatedBiases(null));
12286         assertNull(calibrator.getEstimatedBiasesAsMatrix());
12287         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12288         assertNull(calibrator.getEstimatedBiasFx());
12289         assertNull(calibrator.getEstimatedBiasFy());
12290         assertNull(calibrator.getEstimatedBiasFz());
12291         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12292         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12293         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12294         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12295         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12296         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12297         assertNull(calibrator.getEstimatedMa());
12298         assertNull(calibrator.getEstimatedSx());
12299         assertNull(calibrator.getEstimatedSy());
12300         assertNull(calibrator.getEstimatedSz());
12301         assertNull(calibrator.getEstimatedMxy());
12302         assertNull(calibrator.getEstimatedMxz());
12303         assertNull(calibrator.getEstimatedMyx());
12304         assertNull(calibrator.getEstimatedMyz());
12305         assertNull(calibrator.getEstimatedMzx());
12306         assertNull(calibrator.getEstimatedMzy());
12307         assertNull(calibrator.getEstimatedCovariance());
12308         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12309         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12310                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12311         assertNotNull(calibrator.getGroundTruthGravityNorm());
12312         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12313         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12314         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12315                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12316         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12317         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12318         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12319     }
12320 
12321     @Test
12322     public void testConstructor104() throws WrongSizeException {
12323         final Collection<StandardDeviationBodyKinematics> measurements =
12324                 Collections.emptyList();
12325 
12326         final Matrix ba = generateBa();
12327         final double biasX = ba.getElementAtIndex(0);
12328         final double biasY = ba.getElementAtIndex(1);
12329         final double biasZ = ba.getElementAtIndex(2);
12330 
12331         final Matrix ma = generateMaCommonAxis();
12332         final double sx = ma.getElementAt(0, 0);
12333         final double sy = ma.getElementAt(1, 1);
12334         final double sz = ma.getElementAt(2, 2);
12335 
12336         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12337         final double latitude = Math.toRadians(
12338                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12339         final double longitude = Math.toRadians(
12340                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12341         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12342         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12343         final NEDVelocity nedVelocity = new NEDVelocity();
12344         final ECEFPosition ecefPosition = new ECEFPosition();
12345         final ECEFVelocity ecefVelocity = new ECEFVelocity();
12346         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12347                 ecefPosition, ecefVelocity);
12348 
12349         final KnownPositionAccelerometerCalibrator calibrator =
12350                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
12351                         biasX, biasY, biasZ, sx, sy, sz);
12352 
12353         // check default values
12354         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12355         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12356         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12357         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12358         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12359         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12360         final Acceleration bx2 = new Acceleration(0.0,
12361                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12362         calibrator.getInitialBiasXAsAcceleration(bx2);
12363         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12364         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12365         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12366         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12367         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12368         final Acceleration by2 = new Acceleration(0.0,
12369                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12370         calibrator.getInitialBiasYAsAcceleration(by2);
12371         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12372         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12373         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12374         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12375         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12376         final Acceleration bz2 = new Acceleration(0.0,
12377                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12378         calibrator.getInitialBiasZAsAcceleration(bz2);
12379         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12380         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12381         assertEquals(calibrator.getInitialSx(), sx, 0.0);
12382         assertEquals(calibrator.getInitialSy(), sy, 0.0);
12383         assertEquals(calibrator.getInitialSz(), sz, 0.0);
12384         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12385         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12386         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12387         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12388         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12389         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12390         final double[] bias1 = calibrator.getInitialBias();
12391         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12392         final double[] bias2 = new double[3];
12393         calibrator.getInitialBias(bias2);
12394         assertArrayEquals(bias1, bias2, 0.0);
12395         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12396         assertEquals(b1, ba);
12397         final Matrix b2 = new Matrix(3, 1);
12398         calibrator.getInitialBiasAsMatrix(b2);
12399         assertEquals(b1, b2);
12400         final Matrix ma1 = calibrator.getInitialMa();
12401         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12402         final Matrix ma2 = new Matrix(3, 3);
12403         calibrator.getInitialMa(ma2);
12404         assertEquals(ma1, ma2);
12405         assertSame(calibrator.getEcefPosition(), ecefPosition);
12406         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12407         final NEDPosition nedPosition1 = new NEDPosition();
12408         assertTrue(calibrator.getNedPosition(nedPosition1));
12409         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12410         assertSame(calibrator.getMeasurements(), measurements);
12411         assertFalse(calibrator.isCommonAxisUsed());
12412         assertNull(calibrator.getListener());
12413         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
12414         assertFalse(calibrator.isReady());
12415         assertFalse(calibrator.isRunning());
12416         assertNull(calibrator.getEstimatedBiases());
12417         assertFalse(calibrator.getEstimatedBiases(null));
12418         assertNull(calibrator.getEstimatedBiasesAsMatrix());
12419         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12420         assertNull(calibrator.getEstimatedBiasFx());
12421         assertNull(calibrator.getEstimatedBiasFy());
12422         assertNull(calibrator.getEstimatedBiasFz());
12423         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12424         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12425         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12426         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12427         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12428         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12429         assertNull(calibrator.getEstimatedMa());
12430         assertNull(calibrator.getEstimatedSx());
12431         assertNull(calibrator.getEstimatedSy());
12432         assertNull(calibrator.getEstimatedSz());
12433         assertNull(calibrator.getEstimatedMxy());
12434         assertNull(calibrator.getEstimatedMxz());
12435         assertNull(calibrator.getEstimatedMyx());
12436         assertNull(calibrator.getEstimatedMyz());
12437         assertNull(calibrator.getEstimatedMzx());
12438         assertNull(calibrator.getEstimatedMzy());
12439         assertNull(calibrator.getEstimatedCovariance());
12440         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12441         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12442                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12443         assertNotNull(calibrator.getGroundTruthGravityNorm());
12444         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12445         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12446         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12447                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12448         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12449         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12450         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12451     }
12452 
12453     @Test
12454     public void testConstructor105() throws WrongSizeException {
12455         final Collection<StandardDeviationBodyKinematics> measurements =
12456                 Collections.emptyList();
12457 
12458         final Matrix ba = generateBa();
12459         final double biasX = ba.getElementAtIndex(0);
12460         final double biasY = ba.getElementAtIndex(1);
12461         final double biasZ = ba.getElementAtIndex(2);
12462 
12463         final Matrix ma = generateMaCommonAxis();
12464         final double sx = ma.getElementAt(0, 0);
12465         final double sy = ma.getElementAt(1, 1);
12466         final double sz = ma.getElementAt(2, 2);
12467 
12468         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12469         final double latitude = Math.toRadians(
12470                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12471         final double longitude = Math.toRadians(
12472                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12473         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12474         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12475         final NEDVelocity nedVelocity = new NEDVelocity();
12476         final ECEFPosition ecefPosition = new ECEFPosition();
12477         final ECEFVelocity ecefVelocity = new ECEFVelocity();
12478         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12479                 ecefPosition, ecefVelocity);
12480 
12481         final KnownPositionAccelerometerCalibrator calibrator =
12482                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
12483                         biasX, biasY, biasZ, sx, sy, sz, this);
12484 
12485         // check default values
12486         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12487         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12488         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12489         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12490         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12491         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12492         final Acceleration bx2 = new Acceleration(0.0,
12493                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12494         calibrator.getInitialBiasXAsAcceleration(bx2);
12495         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12496         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12497         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12498         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12499         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12500         final Acceleration by2 = new Acceleration(0.0,
12501                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12502         calibrator.getInitialBiasYAsAcceleration(by2);
12503         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12504         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12505         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12506         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12507         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12508         final Acceleration bz2 = new Acceleration(0.0,
12509                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12510         calibrator.getInitialBiasZAsAcceleration(bz2);
12511         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12512         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12513         assertEquals(calibrator.getInitialSx(), sx, 0.0);
12514         assertEquals(calibrator.getInitialSy(), sy, 0.0);
12515         assertEquals(calibrator.getInitialSz(), sz, 0.0);
12516         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12517         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12518         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12519         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12520         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12521         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12522         final double[] bias1 = calibrator.getInitialBias();
12523         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12524         final double[] bias2 = new double[3];
12525         calibrator.getInitialBias(bias2);
12526         assertArrayEquals(bias1, bias2, 0.0);
12527         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12528         assertEquals(b1, ba);
12529         final Matrix b2 = new Matrix(3, 1);
12530         calibrator.getInitialBiasAsMatrix(b2);
12531         assertEquals(b1, b2);
12532         final Matrix ma1 = calibrator.getInitialMa();
12533         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12534         final Matrix ma2 = new Matrix(3, 3);
12535         calibrator.getInitialMa(ma2);
12536         assertEquals(ma1, ma2);
12537         assertSame(calibrator.getEcefPosition(), ecefPosition);
12538         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12539         final NEDPosition nedPosition1 = new NEDPosition();
12540         assertTrue(calibrator.getNedPosition(nedPosition1));
12541         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12542         assertSame(calibrator.getMeasurements(), measurements);
12543         assertFalse(calibrator.isCommonAxisUsed());
12544         assertSame(calibrator.getListener(), this);
12545         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
12546         assertFalse(calibrator.isReady());
12547         assertFalse(calibrator.isRunning());
12548         assertNull(calibrator.getEstimatedBiases());
12549         assertFalse(calibrator.getEstimatedBiases(null));
12550         assertNull(calibrator.getEstimatedBiasesAsMatrix());
12551         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12552         assertNull(calibrator.getEstimatedBiasFx());
12553         assertNull(calibrator.getEstimatedBiasFy());
12554         assertNull(calibrator.getEstimatedBiasFz());
12555         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12556         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12557         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12558         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12559         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12560         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12561         assertNull(calibrator.getEstimatedMa());
12562         assertNull(calibrator.getEstimatedSx());
12563         assertNull(calibrator.getEstimatedSy());
12564         assertNull(calibrator.getEstimatedSz());
12565         assertNull(calibrator.getEstimatedMxy());
12566         assertNull(calibrator.getEstimatedMxz());
12567         assertNull(calibrator.getEstimatedMyx());
12568         assertNull(calibrator.getEstimatedMyz());
12569         assertNull(calibrator.getEstimatedMzx());
12570         assertNull(calibrator.getEstimatedMzy());
12571         assertNull(calibrator.getEstimatedCovariance());
12572         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12573         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12574                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12575         assertNotNull(calibrator.getGroundTruthGravityNorm());
12576         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12577         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12578         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12579                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12580         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12581         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12582         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12583     }
12584 
12585     @Test
12586     public void testConstructor106() throws WrongSizeException {
12587         final Matrix ba = generateBa();
12588         final double biasX = ba.getElementAtIndex(0);
12589         final double biasY = ba.getElementAtIndex(1);
12590         final double biasZ = ba.getElementAtIndex(2);
12591 
12592         final Matrix ma = generateMaCommonAxis();
12593         final double sx = ma.getElementAt(0, 0);
12594         final double sy = ma.getElementAt(1, 1);
12595         final double sz = ma.getElementAt(2, 2);
12596 
12597         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12598         final double latitude = Math.toRadians(
12599                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12600         final double longitude = Math.toRadians(
12601                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12602         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12603         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12604         final NEDVelocity nedVelocity = new NEDVelocity();
12605         final ECEFPosition ecefPosition = new ECEFPosition();
12606         final ECEFVelocity ecefVelocity = new ECEFVelocity();
12607         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12608                 ecefPosition, ecefVelocity);
12609 
12610         final KnownPositionAccelerometerCalibrator calibrator =
12611                 new KnownPositionAccelerometerCalibrator(ecefPosition,
12612                         true, biasX, biasY, biasZ, sx, sy, sz);
12613 
12614         // check default values
12615         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12616         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12617         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12618         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12619         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12620         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12621         final Acceleration bx2 = new Acceleration(0.0,
12622                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12623         calibrator.getInitialBiasXAsAcceleration(bx2);
12624         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12625         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12626         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12627         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12628         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12629         final Acceleration by2 = new Acceleration(0.0,
12630                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12631         calibrator.getInitialBiasYAsAcceleration(by2);
12632         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12633         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12634         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12635         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12636         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12637         final Acceleration bz2 = new Acceleration(0.0,
12638                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12639         calibrator.getInitialBiasZAsAcceleration(bz2);
12640         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12641         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12642         assertEquals(calibrator.getInitialSx(), sx, 0.0);
12643         assertEquals(calibrator.getInitialSy(), sy, 0.0);
12644         assertEquals(calibrator.getInitialSz(), sz, 0.0);
12645         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12646         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12647         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12648         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12649         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12650         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12651         final double[] bias1 = calibrator.getInitialBias();
12652         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12653         final double[] bias2 = new double[3];
12654         calibrator.getInitialBias(bias2);
12655         assertArrayEquals(bias1, bias2, 0.0);
12656         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12657         assertEquals(b1, ba);
12658         final Matrix b2 = new Matrix(3, 1);
12659         calibrator.getInitialBiasAsMatrix(b2);
12660         assertEquals(b1, b2);
12661         final Matrix ma1 = calibrator.getInitialMa();
12662         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12663         final Matrix ma2 = new Matrix(3, 3);
12664         calibrator.getInitialMa(ma2);
12665         assertEquals(ma1, ma2);
12666         assertSame(calibrator.getEcefPosition(), ecefPosition);
12667         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12668         final NEDPosition nedPosition1 = new NEDPosition();
12669         assertTrue(calibrator.getNedPosition(nedPosition1));
12670         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12671         assertNull(calibrator.getMeasurements());
12672         assertTrue(calibrator.isCommonAxisUsed());
12673         assertNull(calibrator.getListener());
12674         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12675         assertFalse(calibrator.isReady());
12676         assertFalse(calibrator.isRunning());
12677         assertNull(calibrator.getEstimatedBiases());
12678         assertFalse(calibrator.getEstimatedBiases(null));
12679         assertNull(calibrator.getEstimatedBiasesAsMatrix());
12680         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12681         assertNull(calibrator.getEstimatedBiasFx());
12682         assertNull(calibrator.getEstimatedBiasFy());
12683         assertNull(calibrator.getEstimatedBiasFz());
12684         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12685         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12686         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12687         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12688         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12689         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12690         assertNull(calibrator.getEstimatedMa());
12691         assertNull(calibrator.getEstimatedSx());
12692         assertNull(calibrator.getEstimatedSy());
12693         assertNull(calibrator.getEstimatedSz());
12694         assertNull(calibrator.getEstimatedMxy());
12695         assertNull(calibrator.getEstimatedMxz());
12696         assertNull(calibrator.getEstimatedMyx());
12697         assertNull(calibrator.getEstimatedMyz());
12698         assertNull(calibrator.getEstimatedMzx());
12699         assertNull(calibrator.getEstimatedMzy());
12700         assertNull(calibrator.getEstimatedCovariance());
12701         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12702         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12703                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12704         assertNotNull(calibrator.getGroundTruthGravityNorm());
12705         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12706         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12707         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12708                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12709         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12710         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12711         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12712     }
12713 
12714     @Test
12715     public void testConstructor107() throws WrongSizeException {
12716         final Matrix ba = generateBa();
12717         final double biasX = ba.getElementAtIndex(0);
12718         final double biasY = ba.getElementAtIndex(1);
12719         final double biasZ = ba.getElementAtIndex(2);
12720 
12721         final Matrix ma = generateMaCommonAxis();
12722         final double sx = ma.getElementAt(0, 0);
12723         final double sy = ma.getElementAt(1, 1);
12724         final double sz = ma.getElementAt(2, 2);
12725 
12726         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12727         final double latitude = Math.toRadians(
12728                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12729         final double longitude = Math.toRadians(
12730                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12731         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12732         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12733         final NEDVelocity nedVelocity = new NEDVelocity();
12734         final ECEFPosition ecefPosition = new ECEFPosition();
12735         final ECEFVelocity ecefVelocity = new ECEFVelocity();
12736         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12737                 ecefPosition, ecefVelocity);
12738 
12739         final KnownPositionAccelerometerCalibrator calibrator =
12740                 new KnownPositionAccelerometerCalibrator(ecefPosition,
12741                         true, biasX, biasY, biasZ, sx, sy, sz,
12742                         this);
12743 
12744         // check default values
12745         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12746         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12747         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12748         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12749         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12750         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12751         final Acceleration bx2 = new Acceleration(0.0,
12752                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12753         calibrator.getInitialBiasXAsAcceleration(bx2);
12754         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12755         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12756         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12757         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12758         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12759         final Acceleration by2 = new Acceleration(0.0,
12760                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12761         calibrator.getInitialBiasYAsAcceleration(by2);
12762         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12763         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12764         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12765         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12766         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12767         final Acceleration bz2 = new Acceleration(0.0,
12768                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12769         calibrator.getInitialBiasZAsAcceleration(bz2);
12770         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12771         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12772         assertEquals(calibrator.getInitialSx(), sx, 0.0);
12773         assertEquals(calibrator.getInitialSy(), sy, 0.0);
12774         assertEquals(calibrator.getInitialSz(), sz, 0.0);
12775         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12776         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12777         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12778         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12779         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12780         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12781         final double[] bias1 = calibrator.getInitialBias();
12782         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12783         final double[] bias2 = new double[3];
12784         calibrator.getInitialBias(bias2);
12785         assertArrayEquals(bias1, bias2, 0.0);
12786         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12787         assertEquals(b1, ba);
12788         final Matrix b2 = new Matrix(3, 1);
12789         calibrator.getInitialBiasAsMatrix(b2);
12790         assertEquals(b1, b2);
12791         final Matrix ma1 = calibrator.getInitialMa();
12792         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12793         final Matrix ma2 = new Matrix(3, 3);
12794         calibrator.getInitialMa(ma2);
12795         assertEquals(ma1, ma2);
12796         assertSame(calibrator.getEcefPosition(), ecefPosition);
12797         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12798         final NEDPosition nedPosition1 = new NEDPosition();
12799         assertTrue(calibrator.getNedPosition(nedPosition1));
12800         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12801         assertNull(calibrator.getMeasurements());
12802         assertTrue(calibrator.isCommonAxisUsed());
12803         assertSame(calibrator.getListener(), this);
12804         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12805         assertFalse(calibrator.isReady());
12806         assertFalse(calibrator.isRunning());
12807         assertNull(calibrator.getEstimatedBiases());
12808         assertFalse(calibrator.getEstimatedBiases(null));
12809         assertNull(calibrator.getEstimatedBiasesAsMatrix());
12810         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12811         assertNull(calibrator.getEstimatedBiasFx());
12812         assertNull(calibrator.getEstimatedBiasFy());
12813         assertNull(calibrator.getEstimatedBiasFz());
12814         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12815         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12816         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12817         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12818         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12819         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12820         assertNull(calibrator.getEstimatedMa());
12821         assertNull(calibrator.getEstimatedSx());
12822         assertNull(calibrator.getEstimatedSy());
12823         assertNull(calibrator.getEstimatedSz());
12824         assertNull(calibrator.getEstimatedMxy());
12825         assertNull(calibrator.getEstimatedMxz());
12826         assertNull(calibrator.getEstimatedMyx());
12827         assertNull(calibrator.getEstimatedMyz());
12828         assertNull(calibrator.getEstimatedMzx());
12829         assertNull(calibrator.getEstimatedMzy());
12830         assertNull(calibrator.getEstimatedCovariance());
12831         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12832         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12833                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12834         assertNotNull(calibrator.getGroundTruthGravityNorm());
12835         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12836         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12837         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12838                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12839         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12840         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12841         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12842     }
12843 
12844     @Test
12845     public void testConstructor108() throws WrongSizeException {
12846         final Collection<StandardDeviationBodyKinematics> measurements =
12847                 Collections.emptyList();
12848 
12849         final Matrix ba = generateBa();
12850         final double biasX = ba.getElementAtIndex(0);
12851         final double biasY = ba.getElementAtIndex(1);
12852         final double biasZ = ba.getElementAtIndex(2);
12853 
12854         final Matrix ma = generateMaCommonAxis();
12855         final double sx = ma.getElementAt(0, 0);
12856         final double sy = ma.getElementAt(1, 1);
12857         final double sz = ma.getElementAt(2, 2);
12858 
12859         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12860         final double latitude = Math.toRadians(
12861                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12862         final double longitude = Math.toRadians(
12863                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12864         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12865         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12866         final NEDVelocity nedVelocity = new NEDVelocity();
12867         final ECEFPosition ecefPosition = new ECEFPosition();
12868         final ECEFVelocity ecefVelocity = new ECEFVelocity();
12869         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12870                 ecefPosition, ecefVelocity);
12871 
12872         final KnownPositionAccelerometerCalibrator calibrator =
12873                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
12874                         true, biasX, biasY, biasZ, sx, sy, sz);
12875 
12876         // check default values
12877         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12878         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12879         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12880         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12881         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12882         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12883         final Acceleration bx2 = new Acceleration(0.0,
12884                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12885         calibrator.getInitialBiasXAsAcceleration(bx2);
12886         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12887         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12888         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12889         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12890         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12891         final Acceleration by2 = new Acceleration(0.0,
12892                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12893         calibrator.getInitialBiasYAsAcceleration(by2);
12894         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12895         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12896         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12897         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12898         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12899         final Acceleration bz2 = new Acceleration(0.0,
12900                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12901         calibrator.getInitialBiasZAsAcceleration(bz2);
12902         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12903         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12904         assertEquals(calibrator.getInitialSx(), sx, 0.0);
12905         assertEquals(calibrator.getInitialSy(), sy, 0.0);
12906         assertEquals(calibrator.getInitialSz(), sz, 0.0);
12907         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12908         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12909         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12910         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12911         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12912         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12913         final double[] bias1 = calibrator.getInitialBias();
12914         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12915         final double[] bias2 = new double[3];
12916         calibrator.getInitialBias(bias2);
12917         assertArrayEquals(bias1, bias2, 0.0);
12918         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12919         assertEquals(b1, ba);
12920         final Matrix b2 = new Matrix(3, 1);
12921         calibrator.getInitialBiasAsMatrix(b2);
12922         assertEquals(b1, b2);
12923         final Matrix ma1 = calibrator.getInitialMa();
12924         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12925         final Matrix ma2 = new Matrix(3, 3);
12926         calibrator.getInitialMa(ma2);
12927         assertEquals(ma1, ma2);
12928         assertSame(calibrator.getEcefPosition(), ecefPosition);
12929         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12930         final NEDPosition nedPosition1 = new NEDPosition();
12931         assertTrue(calibrator.getNedPosition(nedPosition1));
12932         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12933         assertSame(calibrator.getMeasurements(), measurements);
12934         assertTrue(calibrator.isCommonAxisUsed());
12935         assertNull(calibrator.getListener());
12936         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12937         assertFalse(calibrator.isReady());
12938         assertFalse(calibrator.isRunning());
12939         assertNull(calibrator.getEstimatedBiases());
12940         assertFalse(calibrator.getEstimatedBiases(null));
12941         assertNull(calibrator.getEstimatedBiasesAsMatrix());
12942         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12943         assertNull(calibrator.getEstimatedBiasFx());
12944         assertNull(calibrator.getEstimatedBiasFy());
12945         assertNull(calibrator.getEstimatedBiasFz());
12946         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12947         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12948         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12949         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12950         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12951         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12952         assertNull(calibrator.getEstimatedMa());
12953         assertNull(calibrator.getEstimatedSx());
12954         assertNull(calibrator.getEstimatedSy());
12955         assertNull(calibrator.getEstimatedSz());
12956         assertNull(calibrator.getEstimatedMxy());
12957         assertNull(calibrator.getEstimatedMxz());
12958         assertNull(calibrator.getEstimatedMyx());
12959         assertNull(calibrator.getEstimatedMyz());
12960         assertNull(calibrator.getEstimatedMzx());
12961         assertNull(calibrator.getEstimatedMzy());
12962         assertNull(calibrator.getEstimatedCovariance());
12963         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12964         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12965                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12966         assertNotNull(calibrator.getGroundTruthGravityNorm());
12967         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12968         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12969         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12970                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12971         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12972         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12973         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12974     }
12975 
12976     @Test
12977     public void testConstructor109() throws WrongSizeException {
12978         final Collection<StandardDeviationBodyKinematics> measurements =
12979                 Collections.emptyList();
12980 
12981         final Matrix ba = generateBa();
12982         final double biasX = ba.getElementAtIndex(0);
12983         final double biasY = ba.getElementAtIndex(1);
12984         final double biasZ = ba.getElementAtIndex(2);
12985 
12986         final Matrix ma = generateMaCommonAxis();
12987         final double sx = ma.getElementAt(0, 0);
12988         final double sy = ma.getElementAt(1, 1);
12989         final double sz = ma.getElementAt(2, 2);
12990 
12991         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12992         final double latitude = Math.toRadians(
12993                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12994         final double longitude = Math.toRadians(
12995                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12996         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12997         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12998         final NEDVelocity nedVelocity = new NEDVelocity();
12999         final ECEFPosition ecefPosition = new ECEFPosition();
13000         final ECEFVelocity ecefVelocity = new ECEFVelocity();
13001         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13002                 ecefPosition, ecefVelocity);
13003 
13004         final KnownPositionAccelerometerCalibrator calibrator =
13005                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
13006                         true, biasX, biasY, biasZ, sx, sy, sz,
13007                         this);
13008 
13009         // check default values
13010         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13011         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13012         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13013         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13014         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13015         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13016         final Acceleration bx2 = new Acceleration(0.0,
13017                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13018         calibrator.getInitialBiasXAsAcceleration(bx2);
13019         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13020         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13021         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13022         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13023         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13024         final Acceleration by2 = new Acceleration(0.0,
13025                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13026         calibrator.getInitialBiasYAsAcceleration(by2);
13027         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13028         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13029         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13030         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13031         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13032         final Acceleration bz2 = new Acceleration(0.0,
13033                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13034         calibrator.getInitialBiasZAsAcceleration(bz2);
13035         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13036         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13037         assertEquals(calibrator.getInitialSx(), sx, 0.0);
13038         assertEquals(calibrator.getInitialSy(), sy, 0.0);
13039         assertEquals(calibrator.getInitialSz(), sz, 0.0);
13040         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13041         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13042         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13043         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13044         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13045         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13046         final double[] bias1 = calibrator.getInitialBias();
13047         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13048         final double[] bias2 = new double[3];
13049         calibrator.getInitialBias(bias2);
13050         assertArrayEquals(bias1, bias2, 0.0);
13051         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13052         assertEquals(b1, ba);
13053         final Matrix b2 = new Matrix(3, 1);
13054         calibrator.getInitialBiasAsMatrix(b2);
13055         assertEquals(b1, b2);
13056         final Matrix ma1 = calibrator.getInitialMa();
13057         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13058         final Matrix ma2 = new Matrix(3, 3);
13059         calibrator.getInitialMa(ma2);
13060         assertEquals(ma1, ma2);
13061         assertSame(calibrator.getEcefPosition(), ecefPosition);
13062         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13063         final NEDPosition nedPosition1 = new NEDPosition();
13064         assertTrue(calibrator.getNedPosition(nedPosition1));
13065         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13066         assertSame(calibrator.getMeasurements(), measurements);
13067         assertTrue(calibrator.isCommonAxisUsed());
13068         assertSame(calibrator.getListener(), this);
13069         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
13070         assertFalse(calibrator.isReady());
13071         assertFalse(calibrator.isRunning());
13072         assertNull(calibrator.getEstimatedBiases());
13073         assertFalse(calibrator.getEstimatedBiases(null));
13074         assertNull(calibrator.getEstimatedBiasesAsMatrix());
13075         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13076         assertNull(calibrator.getEstimatedBiasFx());
13077         assertNull(calibrator.getEstimatedBiasFy());
13078         assertNull(calibrator.getEstimatedBiasFz());
13079         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13080         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13081         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13082         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13083         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13084         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13085         assertNull(calibrator.getEstimatedMa());
13086         assertNull(calibrator.getEstimatedSx());
13087         assertNull(calibrator.getEstimatedSy());
13088         assertNull(calibrator.getEstimatedSz());
13089         assertNull(calibrator.getEstimatedMxy());
13090         assertNull(calibrator.getEstimatedMxz());
13091         assertNull(calibrator.getEstimatedMyx());
13092         assertNull(calibrator.getEstimatedMyz());
13093         assertNull(calibrator.getEstimatedMzx());
13094         assertNull(calibrator.getEstimatedMzy());
13095         assertNull(calibrator.getEstimatedCovariance());
13096         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13097         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13098                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13099         assertNotNull(calibrator.getGroundTruthGravityNorm());
13100         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13101         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13102         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13103                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13104         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13105         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13106         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13107     }
13108 
13109     @Test
13110     public void testConstructor110() throws WrongSizeException {
13111         final Matrix ba = generateBa();
13112         final double biasX = ba.getElementAtIndex(0);
13113         final double biasY = ba.getElementAtIndex(1);
13114         final double biasZ = ba.getElementAtIndex(2);
13115 
13116         final Acceleration bx = new Acceleration(biasX,
13117                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13118         final Acceleration by = new Acceleration(biasY,
13119                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13120         final Acceleration bz = new Acceleration(biasZ,
13121                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13122 
13123         final Matrix ma = generateMaCommonAxis();
13124         final double sx = ma.getElementAt(0, 0);
13125         final double sy = ma.getElementAt(1, 1);
13126         final double sz = ma.getElementAt(2, 2);
13127 
13128         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13129         final double latitude = Math.toRadians(
13130                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13131         final double longitude = Math.toRadians(
13132                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13133         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13134         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13135         final NEDVelocity nedVelocity = new NEDVelocity();
13136         final ECEFPosition ecefPosition = new ECEFPosition();
13137         final ECEFVelocity ecefVelocity = new ECEFVelocity();
13138         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13139                 ecefPosition, ecefVelocity);
13140 
13141         final KnownPositionAccelerometerCalibrator calibrator =
13142                 new KnownPositionAccelerometerCalibrator(ecefPosition,
13143                         bx, by, bz, sx, sy, sz);
13144 
13145         // check default values
13146         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13147         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13148         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13149         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13150         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13151         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13152         final Acceleration bx2 = new Acceleration(0.0,
13153                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13154         calibrator.getInitialBiasXAsAcceleration(bx2);
13155         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13156         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13157         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13158         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13159         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13160         final Acceleration by2 = new Acceleration(0.0,
13161                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13162         calibrator.getInitialBiasYAsAcceleration(by2);
13163         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13164         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13165         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13166         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13167         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13168         final Acceleration bz2 = new Acceleration(0.0,
13169                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13170         calibrator.getInitialBiasZAsAcceleration(bz2);
13171         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13172         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13173         assertEquals(calibrator.getInitialSx(), sx, 0.0);
13174         assertEquals(calibrator.getInitialSy(), sy, 0.0);
13175         assertEquals(calibrator.getInitialSz(), sz, 0.0);
13176         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13177         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13178         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13179         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13180         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13181         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13182         final double[] bias1 = calibrator.getInitialBias();
13183         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13184         final double[] bias2 = new double[3];
13185         calibrator.getInitialBias(bias2);
13186         assertArrayEquals(bias1, bias2, 0.0);
13187         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13188         assertEquals(b1, ba);
13189         final Matrix b2 = new Matrix(3, 1);
13190         calibrator.getInitialBiasAsMatrix(b2);
13191         assertEquals(b1, b2);
13192         final Matrix ma1 = calibrator.getInitialMa();
13193         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13194         final Matrix ma2 = new Matrix(3, 3);
13195         calibrator.getInitialMa(ma2);
13196         assertEquals(ma1, ma2);
13197         assertSame(calibrator.getEcefPosition(), ecefPosition);
13198         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13199         final NEDPosition nedPosition1 = new NEDPosition();
13200         assertTrue(calibrator.getNedPosition(nedPosition1));
13201         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13202         assertNull(calibrator.getMeasurements());
13203         assertFalse(calibrator.isCommonAxisUsed());
13204         assertNull(calibrator.getListener());
13205         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
13206         assertFalse(calibrator.isReady());
13207         assertFalse(calibrator.isRunning());
13208         assertNull(calibrator.getEstimatedBiases());
13209         assertFalse(calibrator.getEstimatedBiases(null));
13210         assertNull(calibrator.getEstimatedBiasesAsMatrix());
13211         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13212         assertNull(calibrator.getEstimatedBiasFx());
13213         assertNull(calibrator.getEstimatedBiasFy());
13214         assertNull(calibrator.getEstimatedBiasFz());
13215         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13216         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13217         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13218         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13219         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13220         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13221         assertNull(calibrator.getEstimatedMa());
13222         assertNull(calibrator.getEstimatedSx());
13223         assertNull(calibrator.getEstimatedSy());
13224         assertNull(calibrator.getEstimatedSz());
13225         assertNull(calibrator.getEstimatedMxy());
13226         assertNull(calibrator.getEstimatedMxz());
13227         assertNull(calibrator.getEstimatedMyx());
13228         assertNull(calibrator.getEstimatedMyz());
13229         assertNull(calibrator.getEstimatedMzx());
13230         assertNull(calibrator.getEstimatedMzy());
13231         assertNull(calibrator.getEstimatedCovariance());
13232         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13233         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13234                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13235         assertNotNull(calibrator.getGroundTruthGravityNorm());
13236         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13237         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13238         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13239                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13240         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13241         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13242         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13243     }
13244 
13245     @Test
13246     public void testConstructor111() throws WrongSizeException {
13247         final Matrix ba = generateBa();
13248         final double biasX = ba.getElementAtIndex(0);
13249         final double biasY = ba.getElementAtIndex(1);
13250         final double biasZ = ba.getElementAtIndex(2);
13251 
13252         final Acceleration bx = new Acceleration(biasX,
13253                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13254         final Acceleration by = new Acceleration(biasY,
13255                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13256         final Acceleration bz = new Acceleration(biasZ,
13257                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13258 
13259         final Matrix ma = generateMaCommonAxis();
13260         final double sx = ma.getElementAt(0, 0);
13261         final double sy = ma.getElementAt(1, 1);
13262         final double sz = ma.getElementAt(2, 2);
13263 
13264         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13265         final double latitude = Math.toRadians(
13266                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13267         final double longitude = Math.toRadians(
13268                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13269         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13270         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13271         final NEDVelocity nedVelocity = new NEDVelocity();
13272         final ECEFPosition ecefPosition = new ECEFPosition();
13273         final ECEFVelocity ecefVelocity = new ECEFVelocity();
13274         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13275                 ecefPosition, ecefVelocity);
13276 
13277         final KnownPositionAccelerometerCalibrator calibrator =
13278                 new KnownPositionAccelerometerCalibrator(ecefPosition,
13279                         bx, by, bz, sx, sy, sz, this);
13280 
13281         // check default values
13282         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13283         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13284         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13285         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13286         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13287         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13288         final Acceleration bx2 = new Acceleration(0.0,
13289                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13290         calibrator.getInitialBiasXAsAcceleration(bx2);
13291         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13292         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13293         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13294         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13295         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13296         final Acceleration by2 = new Acceleration(0.0,
13297                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13298         calibrator.getInitialBiasYAsAcceleration(by2);
13299         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13300         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13301         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13302         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13303         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13304         final Acceleration bz2 = new Acceleration(0.0,
13305                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13306         calibrator.getInitialBiasZAsAcceleration(bz2);
13307         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13308         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13309         assertEquals(calibrator.getInitialSx(), sx, 0.0);
13310         assertEquals(calibrator.getInitialSy(), sy, 0.0);
13311         assertEquals(calibrator.getInitialSz(), sz, 0.0);
13312         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13313         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13314         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13315         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13316         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13317         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13318         final double[] bias1 = calibrator.getInitialBias();
13319         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13320         final double[] bias2 = new double[3];
13321         calibrator.getInitialBias(bias2);
13322         assertArrayEquals(bias1, bias2, 0.0);
13323         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13324         assertEquals(b1, ba);
13325         final Matrix b2 = new Matrix(3, 1);
13326         calibrator.getInitialBiasAsMatrix(b2);
13327         assertEquals(b1, b2);
13328         final Matrix ma1 = calibrator.getInitialMa();
13329         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13330         final Matrix ma2 = new Matrix(3, 3);
13331         calibrator.getInitialMa(ma2);
13332         assertEquals(ma1, ma2);
13333         assertSame(calibrator.getEcefPosition(), ecefPosition);
13334         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13335         final NEDPosition nedPosition1 = new NEDPosition();
13336         assertTrue(calibrator.getNedPosition(nedPosition1));
13337         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13338         assertNull(calibrator.getMeasurements());
13339         assertFalse(calibrator.isCommonAxisUsed());
13340         assertSame(calibrator.getListener(), this);
13341         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
13342         assertFalse(calibrator.isReady());
13343         assertFalse(calibrator.isRunning());
13344         assertNull(calibrator.getEstimatedBiases());
13345         assertFalse(calibrator.getEstimatedBiases(null));
13346         assertNull(calibrator.getEstimatedBiasesAsMatrix());
13347         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13348         assertNull(calibrator.getEstimatedBiasFx());
13349         assertNull(calibrator.getEstimatedBiasFy());
13350         assertNull(calibrator.getEstimatedBiasFz());
13351         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13352         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13353         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13354         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13355         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13356         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13357         assertNull(calibrator.getEstimatedMa());
13358         assertNull(calibrator.getEstimatedSx());
13359         assertNull(calibrator.getEstimatedSy());
13360         assertNull(calibrator.getEstimatedSz());
13361         assertNull(calibrator.getEstimatedMxy());
13362         assertNull(calibrator.getEstimatedMxz());
13363         assertNull(calibrator.getEstimatedMyx());
13364         assertNull(calibrator.getEstimatedMyz());
13365         assertNull(calibrator.getEstimatedMzx());
13366         assertNull(calibrator.getEstimatedMzy());
13367         assertNull(calibrator.getEstimatedCovariance());
13368         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13369         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13370                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13371         assertNotNull(calibrator.getGroundTruthGravityNorm());
13372         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13373         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13374         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13375                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13376         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13377         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13378         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13379     }
13380 
13381     @Test
13382     public void testConstructor112() throws WrongSizeException {
13383         final Collection<StandardDeviationBodyKinematics> measurements =
13384                 Collections.emptyList();
13385 
13386         final Matrix ba = generateBa();
13387         final double biasX = ba.getElementAtIndex(0);
13388         final double biasY = ba.getElementAtIndex(1);
13389         final double biasZ = ba.getElementAtIndex(2);
13390 
13391         final Acceleration bx = new Acceleration(biasX,
13392                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13393         final Acceleration by = new Acceleration(biasY,
13394                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13395         final Acceleration bz = new Acceleration(biasZ,
13396                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13397 
13398         final Matrix ma = generateMaCommonAxis();
13399         final double sx = ma.getElementAt(0, 0);
13400         final double sy = ma.getElementAt(1, 1);
13401         final double sz = ma.getElementAt(2, 2);
13402 
13403         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13404         final double latitude = Math.toRadians(
13405                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13406         final double longitude = Math.toRadians(
13407                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13408         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13409         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13410         final NEDVelocity nedVelocity = new NEDVelocity();
13411         final ECEFPosition ecefPosition = new ECEFPosition();
13412         final ECEFVelocity ecefVelocity = new ECEFVelocity();
13413         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13414                 ecefPosition, ecefVelocity);
13415 
13416         final KnownPositionAccelerometerCalibrator calibrator =
13417                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
13418                         bx, by, bz, sx, sy, sz);
13419 
13420         // check default values
13421         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13422         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13423         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13424         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13425         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13426         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13427         final Acceleration bx2 = new Acceleration(0.0,
13428                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13429         calibrator.getInitialBiasXAsAcceleration(bx2);
13430         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13431         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13432         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13433         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13434         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13435         final Acceleration by2 = new Acceleration(0.0,
13436                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13437         calibrator.getInitialBiasYAsAcceleration(by2);
13438         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13439         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13440         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13441         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13442         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13443         final Acceleration bz2 = new Acceleration(0.0,
13444                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13445         calibrator.getInitialBiasZAsAcceleration(bz2);
13446         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13447         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13448         assertEquals(calibrator.getInitialSx(), sx, 0.0);
13449         assertEquals(calibrator.getInitialSy(), sy, 0.0);
13450         assertEquals(calibrator.getInitialSz(), sz, 0.0);
13451         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13452         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13453         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13454         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13455         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13456         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13457         final double[] bias1 = calibrator.getInitialBias();
13458         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13459         final double[] bias2 = new double[3];
13460         calibrator.getInitialBias(bias2);
13461         assertArrayEquals(bias1, bias2, 0.0);
13462         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13463         assertEquals(b1, ba);
13464         final Matrix b2 = new Matrix(3, 1);
13465         calibrator.getInitialBiasAsMatrix(b2);
13466         assertEquals(b1, b2);
13467         final Matrix ma1 = calibrator.getInitialMa();
13468         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13469         final Matrix ma2 = new Matrix(3, 3);
13470         calibrator.getInitialMa(ma2);
13471         assertEquals(ma1, ma2);
13472         assertSame(calibrator.getEcefPosition(), ecefPosition);
13473         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13474         final NEDPosition nedPosition1 = new NEDPosition();
13475         assertTrue(calibrator.getNedPosition(nedPosition1));
13476         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13477         assertSame(calibrator.getMeasurements(), measurements);
13478         assertFalse(calibrator.isCommonAxisUsed());
13479         assertNull(calibrator.getListener());
13480         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
13481         assertFalse(calibrator.isReady());
13482         assertFalse(calibrator.isRunning());
13483         assertNull(calibrator.getEstimatedBiases());
13484         assertFalse(calibrator.getEstimatedBiases(null));
13485         assertNull(calibrator.getEstimatedBiasesAsMatrix());
13486         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13487         assertNull(calibrator.getEstimatedBiasFx());
13488         assertNull(calibrator.getEstimatedBiasFy());
13489         assertNull(calibrator.getEstimatedBiasFz());
13490         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13491         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13492         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13493         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13494         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13495         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13496         assertNull(calibrator.getEstimatedMa());
13497         assertNull(calibrator.getEstimatedSx());
13498         assertNull(calibrator.getEstimatedSy());
13499         assertNull(calibrator.getEstimatedSz());
13500         assertNull(calibrator.getEstimatedMxy());
13501         assertNull(calibrator.getEstimatedMxz());
13502         assertNull(calibrator.getEstimatedMyx());
13503         assertNull(calibrator.getEstimatedMyz());
13504         assertNull(calibrator.getEstimatedMzx());
13505         assertNull(calibrator.getEstimatedMzy());
13506         assertNull(calibrator.getEstimatedCovariance());
13507         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13508         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13509                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13510         assertNotNull(calibrator.getGroundTruthGravityNorm());
13511         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13512         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13513         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13514                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13515         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13516         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13517         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13518     }
13519 
13520     @Test
13521     public void testConstructor113() throws WrongSizeException {
13522         final Collection<StandardDeviationBodyKinematics> measurements =
13523                 Collections.emptyList();
13524 
13525         final Matrix ba = generateBa();
13526         final double biasX = ba.getElementAtIndex(0);
13527         final double biasY = ba.getElementAtIndex(1);
13528         final double biasZ = ba.getElementAtIndex(2);
13529 
13530         final Acceleration bx = new Acceleration(biasX,
13531                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13532         final Acceleration by = new Acceleration(biasY,
13533                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13534         final Acceleration bz = new Acceleration(biasZ,
13535                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13536 
13537         final Matrix ma = generateMaCommonAxis();
13538         final double sx = ma.getElementAt(0, 0);
13539         final double sy = ma.getElementAt(1, 1);
13540         final double sz = ma.getElementAt(2, 2);
13541 
13542         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13543         final double latitude = Math.toRadians(
13544                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13545         final double longitude = Math.toRadians(
13546                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13547         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13548         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13549         final NEDVelocity nedVelocity = new NEDVelocity();
13550         final ECEFPosition ecefPosition = new ECEFPosition();
13551         final ECEFVelocity ecefVelocity = new ECEFVelocity();
13552         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13553                 ecefPosition, ecefVelocity);
13554 
13555         final KnownPositionAccelerometerCalibrator calibrator =
13556                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
13557                         bx, by, bz, sx, sy, sz, this);
13558 
13559         // check default values
13560         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13561         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13562         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13563         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13564         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13565         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13566         final Acceleration bx2 = new Acceleration(0.0,
13567                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13568         calibrator.getInitialBiasXAsAcceleration(bx2);
13569         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13570         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13571         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13572         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13573         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13574         final Acceleration by2 = new Acceleration(0.0,
13575                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13576         calibrator.getInitialBiasYAsAcceleration(by2);
13577         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13578         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13579         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13580         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13581         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13582         final Acceleration bz2 = new Acceleration(0.0,
13583                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13584         calibrator.getInitialBiasZAsAcceleration(bz2);
13585         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13586         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13587         assertEquals(calibrator.getInitialSx(), sx, 0.0);
13588         assertEquals(calibrator.getInitialSy(), sy, 0.0);
13589         assertEquals(calibrator.getInitialSz(), sz, 0.0);
13590         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13591         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13592         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13593         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13594         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13595         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13596         final double[] bias1 = calibrator.getInitialBias();
13597         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13598         final double[] bias2 = new double[3];
13599         calibrator.getInitialBias(bias2);
13600         assertArrayEquals(bias1, bias2, 0.0);
13601         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13602         assertEquals(b1, ba);
13603         final Matrix b2 = new Matrix(3, 1);
13604         calibrator.getInitialBiasAsMatrix(b2);
13605         assertEquals(b1, b2);
13606         final Matrix ma1 = calibrator.getInitialMa();
13607         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13608         final Matrix ma2 = new Matrix(3, 3);
13609         calibrator.getInitialMa(ma2);
13610         assertEquals(ma1, ma2);
13611         assertSame(calibrator.getEcefPosition(), ecefPosition);
13612         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13613         final NEDPosition nedPosition1 = new NEDPosition();
13614         assertTrue(calibrator.getNedPosition(nedPosition1));
13615         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13616         assertSame(calibrator.getMeasurements(), measurements);
13617         assertFalse(calibrator.isCommonAxisUsed());
13618         assertSame(calibrator.getListener(), this);
13619         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
13620         assertFalse(calibrator.isReady());
13621         assertFalse(calibrator.isRunning());
13622         assertNull(calibrator.getEstimatedBiases());
13623         assertFalse(calibrator.getEstimatedBiases(null));
13624         assertNull(calibrator.getEstimatedBiasesAsMatrix());
13625         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13626         assertNull(calibrator.getEstimatedBiasFx());
13627         assertNull(calibrator.getEstimatedBiasFy());
13628         assertNull(calibrator.getEstimatedBiasFz());
13629         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13630         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13631         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13632         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13633         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13634         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13635         assertNull(calibrator.getEstimatedMa());
13636         assertNull(calibrator.getEstimatedSx());
13637         assertNull(calibrator.getEstimatedSy());
13638         assertNull(calibrator.getEstimatedSz());
13639         assertNull(calibrator.getEstimatedMxy());
13640         assertNull(calibrator.getEstimatedMxz());
13641         assertNull(calibrator.getEstimatedMyx());
13642         assertNull(calibrator.getEstimatedMyz());
13643         assertNull(calibrator.getEstimatedMzx());
13644         assertNull(calibrator.getEstimatedMzy());
13645         assertNull(calibrator.getEstimatedCovariance());
13646         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13647         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13648                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13649         assertNotNull(calibrator.getGroundTruthGravityNorm());
13650         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13651         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13652         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13653                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13654         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13655         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13656         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13657     }
13658 
13659     @Test
13660     public void testConstructor114() throws WrongSizeException {
13661         final Matrix ba = generateBa();
13662         final double biasX = ba.getElementAtIndex(0);
13663         final double biasY = ba.getElementAtIndex(1);
13664         final double biasZ = ba.getElementAtIndex(2);
13665 
13666         final Acceleration bx = new Acceleration(biasX,
13667                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13668         final Acceleration by = new Acceleration(biasY,
13669                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13670         final Acceleration bz = new Acceleration(biasZ,
13671                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13672 
13673         final Matrix ma = generateMaCommonAxis();
13674         final double sx = ma.getElementAt(0, 0);
13675         final double sy = ma.getElementAt(1, 1);
13676         final double sz = ma.getElementAt(2, 2);
13677 
13678         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13679         final double latitude = Math.toRadians(
13680                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13681         final double longitude = Math.toRadians(
13682                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13683         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13684         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13685         final NEDVelocity nedVelocity = new NEDVelocity();
13686         final ECEFPosition ecefPosition = new ECEFPosition();
13687         final ECEFVelocity ecefVelocity = new ECEFVelocity();
13688         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13689                 ecefPosition, ecefVelocity);
13690 
13691         final KnownPositionAccelerometerCalibrator calibrator =
13692                 new KnownPositionAccelerometerCalibrator(ecefPosition,
13693                         true, bx, by, bz, sx, sy, sz);
13694 
13695         // check default values
13696         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13697         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13698         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13699         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13700         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13701         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13702         final Acceleration bx2 = new Acceleration(0.0,
13703                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13704         calibrator.getInitialBiasXAsAcceleration(bx2);
13705         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13706         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13707         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13708         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13709         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13710         final Acceleration by2 = new Acceleration(0.0,
13711                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13712         calibrator.getInitialBiasYAsAcceleration(by2);
13713         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13714         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13715         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13716         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13717         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13718         final Acceleration bz2 = new Acceleration(0.0,
13719                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13720         calibrator.getInitialBiasZAsAcceleration(bz2);
13721         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13722         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13723         assertEquals(calibrator.getInitialSx(), sx, 0.0);
13724         assertEquals(calibrator.getInitialSy(), sy, 0.0);
13725         assertEquals(calibrator.getInitialSz(), sz, 0.0);
13726         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13727         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13728         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13729         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13730         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13731         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13732         final double[] bias1 = calibrator.getInitialBias();
13733         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13734         final double[] bias2 = new double[3];
13735         calibrator.getInitialBias(bias2);
13736         assertArrayEquals(bias1, bias2, 0.0);
13737         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13738         assertEquals(b1, ba);
13739         final Matrix b2 = new Matrix(3, 1);
13740         calibrator.getInitialBiasAsMatrix(b2);
13741         assertEquals(b1, b2);
13742         final Matrix ma1 = calibrator.getInitialMa();
13743         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13744         final Matrix ma2 = new Matrix(3, 3);
13745         calibrator.getInitialMa(ma2);
13746         assertEquals(ma1, ma2);
13747         assertSame(calibrator.getEcefPosition(), ecefPosition);
13748         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13749         final NEDPosition nedPosition1 = new NEDPosition();
13750         assertTrue(calibrator.getNedPosition(nedPosition1));
13751         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13752         assertNull(calibrator.getMeasurements());
13753         assertTrue(calibrator.isCommonAxisUsed());
13754         assertNull(calibrator.getListener());
13755         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
13756         assertFalse(calibrator.isReady());
13757         assertFalse(calibrator.isRunning());
13758         assertNull(calibrator.getEstimatedBiases());
13759         assertFalse(calibrator.getEstimatedBiases(null));
13760         assertNull(calibrator.getEstimatedBiasesAsMatrix());
13761         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13762         assertNull(calibrator.getEstimatedBiasFx());
13763         assertNull(calibrator.getEstimatedBiasFy());
13764         assertNull(calibrator.getEstimatedBiasFz());
13765         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13766         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13767         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13768         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13769         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13770         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13771         assertNull(calibrator.getEstimatedMa());
13772         assertNull(calibrator.getEstimatedSx());
13773         assertNull(calibrator.getEstimatedSy());
13774         assertNull(calibrator.getEstimatedSz());
13775         assertNull(calibrator.getEstimatedMxy());
13776         assertNull(calibrator.getEstimatedMxz());
13777         assertNull(calibrator.getEstimatedMyx());
13778         assertNull(calibrator.getEstimatedMyz());
13779         assertNull(calibrator.getEstimatedMzx());
13780         assertNull(calibrator.getEstimatedMzy());
13781         assertNull(calibrator.getEstimatedCovariance());
13782         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13783         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13784                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13785         assertNotNull(calibrator.getGroundTruthGravityNorm());
13786         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13787         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13788         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13789                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13790         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13791         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13792         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13793     }
13794 
13795     @Test
13796     public void testConstructor115() throws WrongSizeException {
13797         final Matrix ba = generateBa();
13798         final double biasX = ba.getElementAtIndex(0);
13799         final double biasY = ba.getElementAtIndex(1);
13800         final double biasZ = ba.getElementAtIndex(2);
13801 
13802         final Acceleration bx = new Acceleration(biasX,
13803                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13804         final Acceleration by = new Acceleration(biasY,
13805                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13806         final Acceleration bz = new Acceleration(biasZ,
13807                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13808 
13809         final Matrix ma = generateMaCommonAxis();
13810         final double sx = ma.getElementAt(0, 0);
13811         final double sy = ma.getElementAt(1, 1);
13812         final double sz = ma.getElementAt(2, 2);
13813 
13814         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13815         final double latitude = Math.toRadians(
13816                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13817         final double longitude = Math.toRadians(
13818                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13819         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13820         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13821         final NEDVelocity nedVelocity = new NEDVelocity();
13822         final ECEFPosition ecefPosition = new ECEFPosition();
13823         final ECEFVelocity ecefVelocity = new ECEFVelocity();
13824         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13825                 ecefPosition, ecefVelocity);
13826 
13827         final KnownPositionAccelerometerCalibrator calibrator =
13828                 new KnownPositionAccelerometerCalibrator(ecefPosition,
13829                         true, bx, by, bz, sx, sy, sz, this);
13830 
13831         // check default values
13832         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13833         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13834         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13835         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13836         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13837         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13838         final Acceleration bx2 = new Acceleration(0.0,
13839                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13840         calibrator.getInitialBiasXAsAcceleration(bx2);
13841         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13842         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13843         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13844         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13845         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13846         final Acceleration by2 = new Acceleration(0.0,
13847                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13848         calibrator.getInitialBiasYAsAcceleration(by2);
13849         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13850         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13851         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13852         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13853         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13854         final Acceleration bz2 = new Acceleration(0.0,
13855                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13856         calibrator.getInitialBiasZAsAcceleration(bz2);
13857         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13858         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13859         assertEquals(calibrator.getInitialSx(), sx, 0.0);
13860         assertEquals(calibrator.getInitialSy(), sy, 0.0);
13861         assertEquals(calibrator.getInitialSz(), sz, 0.0);
13862         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13863         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13864         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13865         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13866         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13867         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13868         final double[] bias1 = calibrator.getInitialBias();
13869         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13870         final double[] bias2 = new double[3];
13871         calibrator.getInitialBias(bias2);
13872         assertArrayEquals(bias1, bias2, 0.0);
13873         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13874         assertEquals(b1, ba);
13875         final Matrix b2 = new Matrix(3, 1);
13876         calibrator.getInitialBiasAsMatrix(b2);
13877         assertEquals(b1, b2);
13878         final Matrix ma1 = calibrator.getInitialMa();
13879         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13880         final Matrix ma2 = new Matrix(3, 3);
13881         calibrator.getInitialMa(ma2);
13882         assertEquals(ma1, ma2);
13883         assertSame(calibrator.getEcefPosition(), ecefPosition);
13884         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13885         final NEDPosition nedPosition1 = new NEDPosition();
13886         assertTrue(calibrator.getNedPosition(nedPosition1));
13887         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13888         assertNull(calibrator.getMeasurements());
13889         assertTrue(calibrator.isCommonAxisUsed());
13890         assertSame(calibrator.getListener(), this);
13891         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
13892         assertFalse(calibrator.isReady());
13893         assertFalse(calibrator.isRunning());
13894         assertNull(calibrator.getEstimatedBiases());
13895         assertFalse(calibrator.getEstimatedBiases(null));
13896         assertNull(calibrator.getEstimatedBiasesAsMatrix());
13897         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13898         assertNull(calibrator.getEstimatedBiasFx());
13899         assertNull(calibrator.getEstimatedBiasFy());
13900         assertNull(calibrator.getEstimatedBiasFz());
13901         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13902         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13903         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13904         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13905         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13906         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13907         assertNull(calibrator.getEstimatedMa());
13908         assertNull(calibrator.getEstimatedSx());
13909         assertNull(calibrator.getEstimatedSy());
13910         assertNull(calibrator.getEstimatedSz());
13911         assertNull(calibrator.getEstimatedMxy());
13912         assertNull(calibrator.getEstimatedMxz());
13913         assertNull(calibrator.getEstimatedMyx());
13914         assertNull(calibrator.getEstimatedMyz());
13915         assertNull(calibrator.getEstimatedMzx());
13916         assertNull(calibrator.getEstimatedMzy());
13917         assertNull(calibrator.getEstimatedCovariance());
13918         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13919         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13920                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13921         assertNotNull(calibrator.getGroundTruthGravityNorm());
13922         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13923         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13924         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13925                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13926         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13927         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13928         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13929     }
13930 
13931     @Test
13932     public void testConstructor116() throws WrongSizeException {
13933         final Collection<StandardDeviationBodyKinematics> measurements =
13934                 Collections.emptyList();
13935 
13936         final Matrix ba = generateBa();
13937         final double biasX = ba.getElementAtIndex(0);
13938         final double biasY = ba.getElementAtIndex(1);
13939         final double biasZ = ba.getElementAtIndex(2);
13940 
13941         final Acceleration bx = new Acceleration(biasX,
13942                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13943         final Acceleration by = new Acceleration(biasY,
13944                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13945         final Acceleration bz = new Acceleration(biasZ,
13946                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13947 
13948         final Matrix ma = generateMaCommonAxis();
13949         final double sx = ma.getElementAt(0, 0);
13950         final double sy = ma.getElementAt(1, 1);
13951         final double sz = ma.getElementAt(2, 2);
13952 
13953         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13954         final double latitude = Math.toRadians(
13955                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13956         final double longitude = Math.toRadians(
13957                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13958         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13959         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13960         final NEDVelocity nedVelocity = new NEDVelocity();
13961         final ECEFPosition ecefPosition = new ECEFPosition();
13962         final ECEFVelocity ecefVelocity = new ECEFVelocity();
13963         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13964                 ecefPosition, ecefVelocity);
13965 
13966         final KnownPositionAccelerometerCalibrator calibrator =
13967                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
13968                         true, bx, by, bz, sx, sy, sz);
13969 
13970         // check default values
13971         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13972         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13973         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13974         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13975         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13976         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13977         final Acceleration bx2 = new Acceleration(0.0,
13978                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13979         calibrator.getInitialBiasXAsAcceleration(bx2);
13980         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13981         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13982         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13983         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13984         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13985         final Acceleration by2 = new Acceleration(0.0,
13986                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13987         calibrator.getInitialBiasYAsAcceleration(by2);
13988         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13989         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13990         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13991         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13992         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13993         final Acceleration bz2 = new Acceleration(0.0,
13994                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13995         calibrator.getInitialBiasZAsAcceleration(bz2);
13996         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13997         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13998         assertEquals(calibrator.getInitialSx(), sx, 0.0);
13999         assertEquals(calibrator.getInitialSy(), sy, 0.0);
14000         assertEquals(calibrator.getInitialSz(), sz, 0.0);
14001         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
14002         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
14003         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
14004         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
14005         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
14006         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
14007         final double[] bias1 = calibrator.getInitialBias();
14008         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14009         final double[] bias2 = new double[3];
14010         calibrator.getInitialBias(bias2);
14011         assertArrayEquals(bias1, bias2, 0.0);
14012         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14013         assertEquals(b1, ba);
14014         final Matrix b2 = new Matrix(3, 1);
14015         calibrator.getInitialBiasAsMatrix(b2);
14016         assertEquals(b1, b2);
14017         final Matrix ma1 = calibrator.getInitialMa();
14018         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
14019         final Matrix ma2 = new Matrix(3, 3);
14020         calibrator.getInitialMa(ma2);
14021         assertEquals(ma1, ma2);
14022         assertSame(calibrator.getEcefPosition(), ecefPosition);
14023         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14024         final NEDPosition nedPosition1 = new NEDPosition();
14025         assertTrue(calibrator.getNedPosition(nedPosition1));
14026         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14027         assertSame(calibrator.getMeasurements(), measurements);
14028         assertTrue(calibrator.isCommonAxisUsed());
14029         assertNull(calibrator.getListener());
14030         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
14031         assertFalse(calibrator.isReady());
14032         assertFalse(calibrator.isRunning());
14033         assertNull(calibrator.getEstimatedBiases());
14034         assertFalse(calibrator.getEstimatedBiases(null));
14035         assertNull(calibrator.getEstimatedBiasesAsMatrix());
14036         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14037         assertNull(calibrator.getEstimatedBiasFx());
14038         assertNull(calibrator.getEstimatedBiasFy());
14039         assertNull(calibrator.getEstimatedBiasFz());
14040         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14041         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14042         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14043         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14044         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14045         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14046         assertNull(calibrator.getEstimatedMa());
14047         assertNull(calibrator.getEstimatedSx());
14048         assertNull(calibrator.getEstimatedSy());
14049         assertNull(calibrator.getEstimatedSz());
14050         assertNull(calibrator.getEstimatedMxy());
14051         assertNull(calibrator.getEstimatedMxz());
14052         assertNull(calibrator.getEstimatedMyx());
14053         assertNull(calibrator.getEstimatedMyz());
14054         assertNull(calibrator.getEstimatedMzx());
14055         assertNull(calibrator.getEstimatedMzy());
14056         assertNull(calibrator.getEstimatedCovariance());
14057         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14058         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14059                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14060         assertNotNull(calibrator.getGroundTruthGravityNorm());
14061         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14062         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14063         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14064                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14065         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14066         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14067         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14068     }
14069 
14070     @Test
14071     public void testConstructor117() throws WrongSizeException {
14072         final Collection<StandardDeviationBodyKinematics> measurements =
14073                 Collections.emptyList();
14074 
14075         final Matrix ba = generateBa();
14076         final double biasX = ba.getElementAtIndex(0);
14077         final double biasY = ba.getElementAtIndex(1);
14078         final double biasZ = ba.getElementAtIndex(2);
14079 
14080         final Acceleration bx = new Acceleration(biasX,
14081                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14082         final Acceleration by = new Acceleration(biasY,
14083                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14084         final Acceleration bz = new Acceleration(biasZ,
14085                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14086 
14087         final Matrix ma = generateMaCommonAxis();
14088         final double sx = ma.getElementAt(0, 0);
14089         final double sy = ma.getElementAt(1, 1);
14090         final double sz = ma.getElementAt(2, 2);
14091 
14092         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14093         final double latitude = Math.toRadians(
14094                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14095         final double longitude = Math.toRadians(
14096                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14097         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14098         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14099         final NEDVelocity nedVelocity = new NEDVelocity();
14100         final ECEFPosition ecefPosition = new ECEFPosition();
14101         final ECEFVelocity ecefVelocity = new ECEFVelocity();
14102         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14103                 ecefPosition, ecefVelocity);
14104 
14105         final KnownPositionAccelerometerCalibrator calibrator =
14106                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
14107                         true, bx, by, bz, sx, sy, sz, this);
14108 
14109         // check default values
14110         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14111         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14112         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14113         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14114         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14115         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14116         final Acceleration bx2 = new Acceleration(0.0,
14117                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14118         calibrator.getInitialBiasXAsAcceleration(bx2);
14119         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14120         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14121         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14122         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14123         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14124         final Acceleration by2 = new Acceleration(0.0,
14125                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14126         calibrator.getInitialBiasYAsAcceleration(by2);
14127         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14128         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14129         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14130         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14131         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14132         final Acceleration bz2 = new Acceleration(0.0,
14133                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14134         calibrator.getInitialBiasZAsAcceleration(bz2);
14135         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14136         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14137         assertEquals(calibrator.getInitialSx(), sx, 0.0);
14138         assertEquals(calibrator.getInitialSy(), sy, 0.0);
14139         assertEquals(calibrator.getInitialSz(), sz, 0.0);
14140         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
14141         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
14142         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
14143         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
14144         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
14145         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
14146         final double[] bias1 = calibrator.getInitialBias();
14147         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14148         final double[] bias2 = new double[3];
14149         calibrator.getInitialBias(bias2);
14150         assertArrayEquals(bias1, bias2, 0.0);
14151         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14152         assertEquals(b1, ba);
14153         final Matrix b2 = new Matrix(3, 1);
14154         calibrator.getInitialBiasAsMatrix(b2);
14155         assertEquals(b1, b2);
14156         final Matrix ma1 = calibrator.getInitialMa();
14157         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
14158         final Matrix ma2 = new Matrix(3, 3);
14159         calibrator.getInitialMa(ma2);
14160         assertEquals(ma1, ma2);
14161         assertSame(calibrator.getEcefPosition(), ecefPosition);
14162         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14163         final NEDPosition nedPosition1 = new NEDPosition();
14164         assertTrue(calibrator.getNedPosition(nedPosition1));
14165         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14166         assertSame(calibrator.getMeasurements(), measurements);
14167         assertTrue(calibrator.isCommonAxisUsed());
14168         assertSame(calibrator.getListener(), this);
14169         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
14170         assertFalse(calibrator.isReady());
14171         assertFalse(calibrator.isRunning());
14172         assertNull(calibrator.getEstimatedBiases());
14173         assertFalse(calibrator.getEstimatedBiases(null));
14174         assertNull(calibrator.getEstimatedBiasesAsMatrix());
14175         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14176         assertNull(calibrator.getEstimatedBiasFx());
14177         assertNull(calibrator.getEstimatedBiasFy());
14178         assertNull(calibrator.getEstimatedBiasFz());
14179         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14180         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14181         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14182         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14183         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14184         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14185         assertNull(calibrator.getEstimatedMa());
14186         assertNull(calibrator.getEstimatedSx());
14187         assertNull(calibrator.getEstimatedSy());
14188         assertNull(calibrator.getEstimatedSz());
14189         assertNull(calibrator.getEstimatedMxy());
14190         assertNull(calibrator.getEstimatedMxz());
14191         assertNull(calibrator.getEstimatedMyx());
14192         assertNull(calibrator.getEstimatedMyz());
14193         assertNull(calibrator.getEstimatedMzx());
14194         assertNull(calibrator.getEstimatedMzy());
14195         assertNull(calibrator.getEstimatedCovariance());
14196         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14197         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14198                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14199         assertNotNull(calibrator.getGroundTruthGravityNorm());
14200         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14201         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14202         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14203                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14204         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14205         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14206         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14207     }
14208 
14209     @Test
14210     public void testConstructor118() throws WrongSizeException {
14211         final Matrix ba = generateBa();
14212         final double biasX = ba.getElementAtIndex(0);
14213         final double biasY = ba.getElementAtIndex(1);
14214         final double biasZ = ba.getElementAtIndex(2);
14215 
14216         final Matrix ma = generateMaCommonAxis();
14217         final double sx = ma.getElementAt(0, 0);
14218         final double sy = ma.getElementAt(1, 1);
14219         final double sz = ma.getElementAt(2, 2);
14220         final double mxy = ma.getElementAt(0, 1);
14221         final double mxz = ma.getElementAt(0, 2);
14222         final double myx = ma.getElementAt(1, 0);
14223         final double myz = ma.getElementAt(1, 2);
14224         final double mzx = ma.getElementAt(2, 0);
14225         final double mzy = ma.getElementAt(2, 1);
14226 
14227         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14228         final double latitude = Math.toRadians(
14229                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14230         final double longitude = Math.toRadians(
14231                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14232         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14233         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14234         final NEDVelocity nedVelocity = new NEDVelocity();
14235         final ECEFPosition ecefPosition = new ECEFPosition();
14236         final ECEFVelocity ecefVelocity = new ECEFVelocity();
14237         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14238                 ecefPosition, ecefVelocity);
14239 
14240         final KnownPositionAccelerometerCalibrator calibrator =
14241                 new KnownPositionAccelerometerCalibrator(ecefPosition,
14242                         biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14243                         myx, myz, mzx, mzy);
14244 
14245         // check default values
14246         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14247         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14248         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14249         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14250         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14251         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14252         final Acceleration bx2 = new Acceleration(0.0,
14253                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14254         calibrator.getInitialBiasXAsAcceleration(bx2);
14255         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14256         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14257         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14258         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14259         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14260         final Acceleration by2 = new Acceleration(0.0,
14261                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14262         calibrator.getInitialBiasYAsAcceleration(by2);
14263         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14264         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14265         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14266         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14267         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14268         final Acceleration bz2 = new Acceleration(0.0,
14269                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14270         calibrator.getInitialBiasZAsAcceleration(bz2);
14271         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14272         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14273         assertEquals(calibrator.getInitialSx(), sx, 0.0);
14274         assertEquals(calibrator.getInitialSy(), sy, 0.0);
14275         assertEquals(calibrator.getInitialSz(), sz, 0.0);
14276         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14277         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14278         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14279         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14280         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14281         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14282         final double[] bias1 = calibrator.getInitialBias();
14283         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14284         final double[] bias2 = new double[3];
14285         calibrator.getInitialBias(bias2);
14286         assertArrayEquals(bias1, bias2, 0.0);
14287         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14288         assertEquals(b1, ba);
14289         final Matrix b2 = new Matrix(3, 1);
14290         calibrator.getInitialBiasAsMatrix(b2);
14291         assertEquals(b1, b2);
14292         final Matrix ma1 = new Matrix(3, 3);
14293         ma1.setSubmatrix(0, 0,
14294                 2, 2,
14295                 new double[]{sx, myx, mzx,
14296                         mxy, sy, mzy,
14297                         mxz, myz, sz});
14298         assertEquals(calibrator.getInitialMa(), ma1);
14299         final Matrix ma2 = new Matrix(3, 3);
14300         calibrator.getInitialMa(ma2);
14301         assertEquals(ma1, ma2);
14302         assertSame(calibrator.getEcefPosition(), ecefPosition);
14303         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14304         final NEDPosition nedPosition1 = new NEDPosition();
14305         assertTrue(calibrator.getNedPosition(nedPosition1));
14306         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14307         assertNull(calibrator.getMeasurements());
14308         assertFalse(calibrator.isCommonAxisUsed());
14309         assertNull(calibrator.getListener());
14310         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
14311         assertFalse(calibrator.isReady());
14312         assertFalse(calibrator.isRunning());
14313         assertNull(calibrator.getEstimatedBiases());
14314         assertFalse(calibrator.getEstimatedBiases(null));
14315         assertNull(calibrator.getEstimatedBiasesAsMatrix());
14316         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14317         assertNull(calibrator.getEstimatedBiasFx());
14318         assertNull(calibrator.getEstimatedBiasFy());
14319         assertNull(calibrator.getEstimatedBiasFz());
14320         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14321         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14322         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14323         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14324         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14325         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14326         assertNull(calibrator.getEstimatedMa());
14327         assertNull(calibrator.getEstimatedSx());
14328         assertNull(calibrator.getEstimatedSy());
14329         assertNull(calibrator.getEstimatedSz());
14330         assertNull(calibrator.getEstimatedMxy());
14331         assertNull(calibrator.getEstimatedMxz());
14332         assertNull(calibrator.getEstimatedMyx());
14333         assertNull(calibrator.getEstimatedMyz());
14334         assertNull(calibrator.getEstimatedMzx());
14335         assertNull(calibrator.getEstimatedMzy());
14336         assertNull(calibrator.getEstimatedCovariance());
14337         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14338         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14339                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14340         assertNotNull(calibrator.getGroundTruthGravityNorm());
14341         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14342         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14343         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14344                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14345         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14346         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14347         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14348     }
14349 
14350     @Test
14351     public void testConstructor119() throws WrongSizeException {
14352         final Collection<StandardDeviationBodyKinematics> measurements =
14353                 Collections.emptyList();
14354 
14355         final Matrix ba = generateBa();
14356         final double biasX = ba.getElementAtIndex(0);
14357         final double biasY = ba.getElementAtIndex(1);
14358         final double biasZ = ba.getElementAtIndex(2);
14359 
14360         final Matrix ma = generateMaCommonAxis();
14361         final double sx = ma.getElementAt(0, 0);
14362         final double sy = ma.getElementAt(1, 1);
14363         final double sz = ma.getElementAt(2, 2);
14364         final double mxy = ma.getElementAt(0, 1);
14365         final double mxz = ma.getElementAt(0, 2);
14366         final double myx = ma.getElementAt(1, 0);
14367         final double myz = ma.getElementAt(1, 2);
14368         final double mzx = ma.getElementAt(2, 0);
14369         final double mzy = ma.getElementAt(2, 1);
14370 
14371         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14372         final double latitude = Math.toRadians(
14373                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14374         final double longitude = Math.toRadians(
14375                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14376         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14377         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14378         final NEDVelocity nedVelocity = new NEDVelocity();
14379         final ECEFPosition ecefPosition = new ECEFPosition();
14380         final ECEFVelocity ecefVelocity = new ECEFVelocity();
14381         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14382                 ecefPosition, ecefVelocity);
14383 
14384         final KnownPositionAccelerometerCalibrator calibrator =
14385                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
14386                         biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14387                         myx, myz, mzx, mzy);
14388 
14389         // check default values
14390         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14391         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14392         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14393         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14394         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14395         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14396         final Acceleration bx2 = new Acceleration(0.0,
14397                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14398         calibrator.getInitialBiasXAsAcceleration(bx2);
14399         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14400         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14401         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14402         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14403         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14404         final Acceleration by2 = new Acceleration(0.0,
14405                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14406         calibrator.getInitialBiasYAsAcceleration(by2);
14407         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14408         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14409         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14410         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14411         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14412         final Acceleration bz2 = new Acceleration(0.0,
14413                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14414         calibrator.getInitialBiasZAsAcceleration(bz2);
14415         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14416         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14417         assertEquals(calibrator.getInitialSx(), sx, 0.0);
14418         assertEquals(calibrator.getInitialSy(), sy, 0.0);
14419         assertEquals(calibrator.getInitialSz(), sz, 0.0);
14420         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14421         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14422         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14423         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14424         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14425         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14426         final double[] bias1 = calibrator.getInitialBias();
14427         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14428         final double[] bias2 = new double[3];
14429         calibrator.getInitialBias(bias2);
14430         assertArrayEquals(bias1, bias2, 0.0);
14431         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14432         assertEquals(b1, ba);
14433         final Matrix b2 = new Matrix(3, 1);
14434         calibrator.getInitialBiasAsMatrix(b2);
14435         assertEquals(b1, b2);
14436         final Matrix ma1 = new Matrix(3, 3);
14437         ma1.setSubmatrix(0, 0,
14438                 2, 2,
14439                 new double[]{sx, myx, mzx,
14440                         mxy, sy, mzy,
14441                         mxz, myz, sz});
14442         assertEquals(calibrator.getInitialMa(), ma1);
14443         final Matrix ma2 = new Matrix(3, 3);
14444         calibrator.getInitialMa(ma2);
14445         assertEquals(ma1, ma2);
14446         assertSame(calibrator.getEcefPosition(), ecefPosition);
14447         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14448         final NEDPosition nedPosition1 = new NEDPosition();
14449         assertTrue(calibrator.getNedPosition(nedPosition1));
14450         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14451         assertSame(calibrator.getMeasurements(), measurements);
14452         assertFalse(calibrator.isCommonAxisUsed());
14453         assertNull(calibrator.getListener());
14454         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
14455         assertFalse(calibrator.isReady());
14456         assertFalse(calibrator.isRunning());
14457         assertNull(calibrator.getEstimatedBiases());
14458         assertFalse(calibrator.getEstimatedBiases(null));
14459         assertNull(calibrator.getEstimatedBiasesAsMatrix());
14460         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14461         assertNull(calibrator.getEstimatedBiasFx());
14462         assertNull(calibrator.getEstimatedBiasFy());
14463         assertNull(calibrator.getEstimatedBiasFz());
14464         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14465         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14466         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14467         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14468         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14469         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14470         assertNull(calibrator.getEstimatedMa());
14471         assertNull(calibrator.getEstimatedSx());
14472         assertNull(calibrator.getEstimatedSy());
14473         assertNull(calibrator.getEstimatedSz());
14474         assertNull(calibrator.getEstimatedMxy());
14475         assertNull(calibrator.getEstimatedMxz());
14476         assertNull(calibrator.getEstimatedMyx());
14477         assertNull(calibrator.getEstimatedMyz());
14478         assertNull(calibrator.getEstimatedMzx());
14479         assertNull(calibrator.getEstimatedMzy());
14480         assertNull(calibrator.getEstimatedCovariance());
14481         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14482         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14483                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14484         assertNotNull(calibrator.getGroundTruthGravityNorm());
14485         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14486         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14487         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14488                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14489         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14490         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14491         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14492     }
14493 
14494     @Test
14495     public void testConstructor120() throws WrongSizeException {
14496         final Collection<StandardDeviationBodyKinematics> measurements =
14497                 Collections.emptyList();
14498 
14499         final Matrix ba = generateBa();
14500         final double biasX = ba.getElementAtIndex(0);
14501         final double biasY = ba.getElementAtIndex(1);
14502         final double biasZ = ba.getElementAtIndex(2);
14503 
14504         final Matrix ma = generateMaCommonAxis();
14505         final double sx = ma.getElementAt(0, 0);
14506         final double sy = ma.getElementAt(1, 1);
14507         final double sz = ma.getElementAt(2, 2);
14508         final double mxy = ma.getElementAt(0, 1);
14509         final double mxz = ma.getElementAt(0, 2);
14510         final double myx = ma.getElementAt(1, 0);
14511         final double myz = ma.getElementAt(1, 2);
14512         final double mzx = ma.getElementAt(2, 0);
14513         final double mzy = ma.getElementAt(2, 1);
14514 
14515         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14516         final double latitude = Math.toRadians(
14517                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14518         final double longitude = Math.toRadians(
14519                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14520         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14521         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14522         final NEDVelocity nedVelocity = new NEDVelocity();
14523         final ECEFPosition ecefPosition = new ECEFPosition();
14524         final ECEFVelocity ecefVelocity = new ECEFVelocity();
14525         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14526                 ecefPosition, ecefVelocity);
14527 
14528         final KnownPositionAccelerometerCalibrator calibrator =
14529                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
14530                         biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14531                         myx, myz, mzx, mzy, this);
14532 
14533         // check default values
14534         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14535         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14536         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14537         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14538         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14539         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14540         final Acceleration bx2 = new Acceleration(0.0,
14541                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14542         calibrator.getInitialBiasXAsAcceleration(bx2);
14543         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14544         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14545         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14546         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14547         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14548         final Acceleration by2 = new Acceleration(0.0,
14549                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14550         calibrator.getInitialBiasYAsAcceleration(by2);
14551         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14552         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14553         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14554         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14555         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14556         final Acceleration bz2 = new Acceleration(0.0,
14557                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14558         calibrator.getInitialBiasZAsAcceleration(bz2);
14559         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14560         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14561         assertEquals(calibrator.getInitialSx(), sx, 0.0);
14562         assertEquals(calibrator.getInitialSy(), sy, 0.0);
14563         assertEquals(calibrator.getInitialSz(), sz, 0.0);
14564         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14565         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14566         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14567         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14568         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14569         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14570         final double[] bias1 = calibrator.getInitialBias();
14571         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14572         final double[] bias2 = new double[3];
14573         calibrator.getInitialBias(bias2);
14574         assertArrayEquals(bias1, bias2, 0.0);
14575         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14576         assertEquals(b1, ba);
14577         final Matrix b2 = new Matrix(3, 1);
14578         calibrator.getInitialBiasAsMatrix(b2);
14579         assertEquals(b1, b2);
14580         final Matrix ma1 = new Matrix(3, 3);
14581         ma1.setSubmatrix(0, 0,
14582                 2, 2,
14583                 new double[]{sx, myx, mzx,
14584                         mxy, sy, mzy,
14585                         mxz, myz, sz});
14586         assertEquals(calibrator.getInitialMa(), ma1);
14587         final Matrix ma2 = new Matrix(3, 3);
14588         calibrator.getInitialMa(ma2);
14589         assertEquals(ma1, ma2);
14590         assertSame(calibrator.getEcefPosition(), ecefPosition);
14591         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14592         final NEDPosition nedPosition1 = new NEDPosition();
14593         assertTrue(calibrator.getNedPosition(nedPosition1));
14594         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14595         assertSame(calibrator.getMeasurements(), measurements);
14596         assertFalse(calibrator.isCommonAxisUsed());
14597         assertSame(calibrator.getListener(), this);
14598         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
14599         assertFalse(calibrator.isReady());
14600         assertFalse(calibrator.isRunning());
14601         assertNull(calibrator.getEstimatedBiases());
14602         assertFalse(calibrator.getEstimatedBiases(null));
14603         assertNull(calibrator.getEstimatedBiasesAsMatrix());
14604         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14605         assertNull(calibrator.getEstimatedBiasFx());
14606         assertNull(calibrator.getEstimatedBiasFy());
14607         assertNull(calibrator.getEstimatedBiasFz());
14608         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14609         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14610         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14611         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14612         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14613         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14614         assertNull(calibrator.getEstimatedMa());
14615         assertNull(calibrator.getEstimatedSx());
14616         assertNull(calibrator.getEstimatedSy());
14617         assertNull(calibrator.getEstimatedSz());
14618         assertNull(calibrator.getEstimatedMxy());
14619         assertNull(calibrator.getEstimatedMxz());
14620         assertNull(calibrator.getEstimatedMyx());
14621         assertNull(calibrator.getEstimatedMyz());
14622         assertNull(calibrator.getEstimatedMzx());
14623         assertNull(calibrator.getEstimatedMzy());
14624         assertNull(calibrator.getEstimatedCovariance());
14625         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14626         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14627                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14628         assertNotNull(calibrator.getGroundTruthGravityNorm());
14629         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14630         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14631         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14632                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14633         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14634         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14635         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14636     }
14637 
14638     @Test
14639     public void testConstructor121() throws WrongSizeException {
14640         final Matrix ba = generateBa();
14641         final double biasX = ba.getElementAtIndex(0);
14642         final double biasY = ba.getElementAtIndex(1);
14643         final double biasZ = ba.getElementAtIndex(2);
14644 
14645         final Matrix ma = generateMaCommonAxis();
14646         final double sx = ma.getElementAt(0, 0);
14647         final double sy = ma.getElementAt(1, 1);
14648         final double sz = ma.getElementAt(2, 2);
14649         final double mxy = ma.getElementAt(0, 1);
14650         final double mxz = ma.getElementAt(0, 2);
14651         final double myx = ma.getElementAt(1, 0);
14652         final double myz = ma.getElementAt(1, 2);
14653         final double mzx = ma.getElementAt(2, 0);
14654         final double mzy = ma.getElementAt(2, 1);
14655 
14656         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14657         final double latitude = Math.toRadians(
14658                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14659         final double longitude = Math.toRadians(
14660                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14661         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14662         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14663         final NEDVelocity nedVelocity = new NEDVelocity();
14664         final ECEFPosition ecefPosition = new ECEFPosition();
14665         final ECEFVelocity ecefVelocity = new ECEFVelocity();
14666         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14667                 ecefPosition, ecefVelocity);
14668 
14669         final KnownPositionAccelerometerCalibrator calibrator =
14670                 new KnownPositionAccelerometerCalibrator(ecefPosition,
14671                         true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14672                         myx, myz, mzx, mzy);
14673 
14674         // check default values
14675         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14676         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14677         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14678         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14679         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14680         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14681         final Acceleration bx2 = new Acceleration(0.0,
14682                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14683         calibrator.getInitialBiasXAsAcceleration(bx2);
14684         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14685         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14686         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14687         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14688         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14689         final Acceleration by2 = new Acceleration(0.0,
14690                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14691         calibrator.getInitialBiasYAsAcceleration(by2);
14692         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14693         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14694         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14695         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14696         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14697         final Acceleration bz2 = new Acceleration(0.0,
14698                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14699         calibrator.getInitialBiasZAsAcceleration(bz2);
14700         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14701         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14702         assertEquals(calibrator.getInitialSx(), sx, 0.0);
14703         assertEquals(calibrator.getInitialSy(), sy, 0.0);
14704         assertEquals(calibrator.getInitialSz(), sz, 0.0);
14705         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14706         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14707         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14708         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14709         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14710         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14711         final double[] bias1 = calibrator.getInitialBias();
14712         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14713         final double[] bias2 = new double[3];
14714         calibrator.getInitialBias(bias2);
14715         assertArrayEquals(bias1, bias2, 0.0);
14716         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14717         assertEquals(b1, ba);
14718         final Matrix b2 = new Matrix(3, 1);
14719         calibrator.getInitialBiasAsMatrix(b2);
14720         assertEquals(b1, b2);
14721         final Matrix ma1 = new Matrix(3, 3);
14722         ma1.setSubmatrix(0, 0,
14723                 2, 2,
14724                 new double[]{sx, myx, mzx,
14725                         mxy, sy, mzy,
14726                         mxz, myz, sz});
14727         assertEquals(calibrator.getInitialMa(), ma1);
14728         final Matrix ma2 = new Matrix(3, 3);
14729         calibrator.getInitialMa(ma2);
14730         assertEquals(ma1, ma2);
14731         assertSame(calibrator.getEcefPosition(), ecefPosition);
14732         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14733         final NEDPosition nedPosition1 = new NEDPosition();
14734         assertTrue(calibrator.getNedPosition(nedPosition1));
14735         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14736         assertNull(calibrator.getMeasurements());
14737         assertTrue(calibrator.isCommonAxisUsed());
14738         assertNull(calibrator.getListener());
14739         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
14740         assertFalse(calibrator.isReady());
14741         assertFalse(calibrator.isRunning());
14742         assertNull(calibrator.getEstimatedBiases());
14743         assertFalse(calibrator.getEstimatedBiases(null));
14744         assertNull(calibrator.getEstimatedBiasesAsMatrix());
14745         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14746         assertNull(calibrator.getEstimatedBiasFx());
14747         assertNull(calibrator.getEstimatedBiasFy());
14748         assertNull(calibrator.getEstimatedBiasFz());
14749         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14750         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14751         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14752         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14753         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14754         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14755         assertNull(calibrator.getEstimatedMa());
14756         assertNull(calibrator.getEstimatedSx());
14757         assertNull(calibrator.getEstimatedSy());
14758         assertNull(calibrator.getEstimatedSz());
14759         assertNull(calibrator.getEstimatedMxy());
14760         assertNull(calibrator.getEstimatedMxz());
14761         assertNull(calibrator.getEstimatedMyx());
14762         assertNull(calibrator.getEstimatedMyz());
14763         assertNull(calibrator.getEstimatedMzx());
14764         assertNull(calibrator.getEstimatedMzy());
14765         assertNull(calibrator.getEstimatedCovariance());
14766         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14767         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14768                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14769         assertNotNull(calibrator.getGroundTruthGravityNorm());
14770         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14771         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14772         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14773                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14774         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14775         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14776         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14777     }
14778 
14779     @Test
14780     public void testConstructor122() throws WrongSizeException {
14781         final Matrix ba = generateBa();
14782         final double biasX = ba.getElementAtIndex(0);
14783         final double biasY = ba.getElementAtIndex(1);
14784         final double biasZ = ba.getElementAtIndex(2);
14785 
14786         final Matrix ma = generateMaCommonAxis();
14787         final double sx = ma.getElementAt(0, 0);
14788         final double sy = ma.getElementAt(1, 1);
14789         final double sz = ma.getElementAt(2, 2);
14790         final double mxy = ma.getElementAt(0, 1);
14791         final double mxz = ma.getElementAt(0, 2);
14792         final double myx = ma.getElementAt(1, 0);
14793         final double myz = ma.getElementAt(1, 2);
14794         final double mzx = ma.getElementAt(2, 0);
14795         final double mzy = ma.getElementAt(2, 1);
14796 
14797         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14798         final double latitude = Math.toRadians(
14799                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14800         final double longitude = Math.toRadians(
14801                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14802         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14803         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14804         final NEDVelocity nedVelocity = new NEDVelocity();
14805         final ECEFPosition ecefPosition = new ECEFPosition();
14806         final ECEFVelocity ecefVelocity = new ECEFVelocity();
14807         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14808                 ecefPosition, ecefVelocity);
14809 
14810         final KnownPositionAccelerometerCalibrator calibrator =
14811                 new KnownPositionAccelerometerCalibrator(ecefPosition,
14812                         true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14813                         myx, myz, mzx, mzy, this);
14814 
14815         // check default values
14816         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14817         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14818         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14819         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14820         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14821         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14822         final Acceleration bx2 = new Acceleration(0.0,
14823                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14824         calibrator.getInitialBiasXAsAcceleration(bx2);
14825         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14826         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14827         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14828         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14829         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14830         final Acceleration by2 = new Acceleration(0.0,
14831                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14832         calibrator.getInitialBiasYAsAcceleration(by2);
14833         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14834         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14835         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14836         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14837         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14838         final Acceleration bz2 = new Acceleration(0.0,
14839                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14840         calibrator.getInitialBiasZAsAcceleration(bz2);
14841         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14842         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14843         assertEquals(calibrator.getInitialSx(), sx, 0.0);
14844         assertEquals(calibrator.getInitialSy(), sy, 0.0);
14845         assertEquals(calibrator.getInitialSz(), sz, 0.0);
14846         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14847         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14848         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14849         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14850         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14851         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14852         final double[] bias1 = calibrator.getInitialBias();
14853         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14854         final double[] bias2 = new double[3];
14855         calibrator.getInitialBias(bias2);
14856         assertArrayEquals(bias1, bias2, 0.0);
14857         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14858         assertEquals(b1, ba);
14859         final Matrix b2 = new Matrix(3, 1);
14860         calibrator.getInitialBiasAsMatrix(b2);
14861         assertEquals(b1, b2);
14862         final Matrix ma1 = new Matrix(3, 3);
14863         ma1.setSubmatrix(0, 0,
14864                 2, 2,
14865                 new double[]{sx, myx, mzx,
14866                         mxy, sy, mzy,
14867                         mxz, myz, sz});
14868         assertEquals(calibrator.getInitialMa(), ma1);
14869         final Matrix ma2 = new Matrix(3, 3);
14870         calibrator.getInitialMa(ma2);
14871         assertEquals(ma1, ma2);
14872         assertSame(calibrator.getEcefPosition(), ecefPosition);
14873         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14874         final NEDPosition nedPosition1 = new NEDPosition();
14875         assertTrue(calibrator.getNedPosition(nedPosition1));
14876         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14877         assertNull(calibrator.getMeasurements());
14878         assertTrue(calibrator.isCommonAxisUsed());
14879         assertSame(calibrator.getListener(), this);
14880         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
14881         assertFalse(calibrator.isReady());
14882         assertFalse(calibrator.isRunning());
14883         assertNull(calibrator.getEstimatedBiases());
14884         assertFalse(calibrator.getEstimatedBiases(null));
14885         assertNull(calibrator.getEstimatedBiasesAsMatrix());
14886         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14887         assertNull(calibrator.getEstimatedBiasFx());
14888         assertNull(calibrator.getEstimatedBiasFy());
14889         assertNull(calibrator.getEstimatedBiasFz());
14890         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14891         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14892         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14893         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14894         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14895         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14896         assertNull(calibrator.getEstimatedMa());
14897         assertNull(calibrator.getEstimatedSx());
14898         assertNull(calibrator.getEstimatedSy());
14899         assertNull(calibrator.getEstimatedSz());
14900         assertNull(calibrator.getEstimatedMxy());
14901         assertNull(calibrator.getEstimatedMxz());
14902         assertNull(calibrator.getEstimatedMyx());
14903         assertNull(calibrator.getEstimatedMyz());
14904         assertNull(calibrator.getEstimatedMzx());
14905         assertNull(calibrator.getEstimatedMzy());
14906         assertNull(calibrator.getEstimatedCovariance());
14907         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14908         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14909                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14910         assertNotNull(calibrator.getGroundTruthGravityNorm());
14911         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14912         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14913         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14914                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14915         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14916         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14917         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14918     }
14919 
14920     @Test
14921     public void testConstructor123() throws WrongSizeException {
14922         final Collection<StandardDeviationBodyKinematics> measurements =
14923                 Collections.emptyList();
14924 
14925         final Matrix ba = generateBa();
14926         final double biasX = ba.getElementAtIndex(0);
14927         final double biasY = ba.getElementAtIndex(1);
14928         final double biasZ = ba.getElementAtIndex(2);
14929 
14930         final Matrix ma = generateMaCommonAxis();
14931         final double sx = ma.getElementAt(0, 0);
14932         final double sy = ma.getElementAt(1, 1);
14933         final double sz = ma.getElementAt(2, 2);
14934         final double mxy = ma.getElementAt(0, 1);
14935         final double mxz = ma.getElementAt(0, 2);
14936         final double myx = ma.getElementAt(1, 0);
14937         final double myz = ma.getElementAt(1, 2);
14938         final double mzx = ma.getElementAt(2, 0);
14939         final double mzy = ma.getElementAt(2, 1);
14940 
14941         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14942         final double latitude = Math.toRadians(
14943                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14944         final double longitude = Math.toRadians(
14945                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14946         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14947         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14948         final NEDVelocity nedVelocity = new NEDVelocity();
14949         final ECEFPosition ecefPosition = new ECEFPosition();
14950         final ECEFVelocity ecefVelocity = new ECEFVelocity();
14951         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14952                 ecefPosition, ecefVelocity);
14953 
14954         final KnownPositionAccelerometerCalibrator calibrator =
14955                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
14956                         true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14957                         myx, myz, mzx, mzy);
14958 
14959         // check default values
14960         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14961         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14962         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14963         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14964         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14965         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14966         final Acceleration bx2 = new Acceleration(0.0,
14967                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14968         calibrator.getInitialBiasXAsAcceleration(bx2);
14969         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14970         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14971         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14972         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14973         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14974         final Acceleration by2 = new Acceleration(0.0,
14975                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14976         calibrator.getInitialBiasYAsAcceleration(by2);
14977         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14978         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14979         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14980         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14981         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14982         final Acceleration bz2 = new Acceleration(0.0,
14983                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14984         calibrator.getInitialBiasZAsAcceleration(bz2);
14985         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14986         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14987         assertEquals(calibrator.getInitialSx(), sx, 0.0);
14988         assertEquals(calibrator.getInitialSy(), sy, 0.0);
14989         assertEquals(calibrator.getInitialSz(), sz, 0.0);
14990         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14991         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14992         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14993         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14994         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14995         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14996         final double[] bias1 = calibrator.getInitialBias();
14997         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14998         final double[] bias2 = new double[3];
14999         calibrator.getInitialBias(bias2);
15000         assertArrayEquals(bias1, bias2, 0.0);
15001         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15002         assertEquals(b1, ba);
15003         final Matrix b2 = new Matrix(3, 1);
15004         calibrator.getInitialBiasAsMatrix(b2);
15005         assertEquals(b1, b2);
15006         final Matrix ma1 = new Matrix(3, 3);
15007         ma1.setSubmatrix(0, 0,
15008                 2, 2,
15009                 new double[]{sx, myx, mzx,
15010                         mxy, sy, mzy,
15011                         mxz, myz, sz});
15012         assertEquals(calibrator.getInitialMa(), ma1);
15013         final Matrix ma2 = new Matrix(3, 3);
15014         calibrator.getInitialMa(ma2);
15015         assertEquals(ma1, ma2);
15016         assertSame(calibrator.getEcefPosition(), ecefPosition);
15017         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15018         final NEDPosition nedPosition1 = new NEDPosition();
15019         assertTrue(calibrator.getNedPosition(nedPosition1));
15020         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15021         assertSame(calibrator.getMeasurements(), measurements);
15022         assertTrue(calibrator.isCommonAxisUsed());
15023         assertNull(calibrator.getListener());
15024         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
15025         assertFalse(calibrator.isReady());
15026         assertFalse(calibrator.isRunning());
15027         assertNull(calibrator.getEstimatedBiases());
15028         assertFalse(calibrator.getEstimatedBiases(null));
15029         assertNull(calibrator.getEstimatedBiasesAsMatrix());
15030         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15031         assertNull(calibrator.getEstimatedBiasFx());
15032         assertNull(calibrator.getEstimatedBiasFy());
15033         assertNull(calibrator.getEstimatedBiasFz());
15034         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15035         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15036         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15037         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15038         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15039         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15040         assertNull(calibrator.getEstimatedMa());
15041         assertNull(calibrator.getEstimatedSx());
15042         assertNull(calibrator.getEstimatedSy());
15043         assertNull(calibrator.getEstimatedSz());
15044         assertNull(calibrator.getEstimatedMxy());
15045         assertNull(calibrator.getEstimatedMxz());
15046         assertNull(calibrator.getEstimatedMyx());
15047         assertNull(calibrator.getEstimatedMyz());
15048         assertNull(calibrator.getEstimatedMzx());
15049         assertNull(calibrator.getEstimatedMzy());
15050         assertNull(calibrator.getEstimatedCovariance());
15051         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15052         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15053                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15054         assertNotNull(calibrator.getGroundTruthGravityNorm());
15055         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15056         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15057         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15058                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15059         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15060         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15061         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15062     }
15063 
15064     @Test
15065     public void testConstructor124() throws WrongSizeException {
15066         final Collection<StandardDeviationBodyKinematics> measurements =
15067                 Collections.emptyList();
15068 
15069         final Matrix ba = generateBa();
15070         final double biasX = ba.getElementAtIndex(0);
15071         final double biasY = ba.getElementAtIndex(1);
15072         final double biasZ = ba.getElementAtIndex(2);
15073 
15074         final Matrix ma = generateMaCommonAxis();
15075         final double sx = ma.getElementAt(0, 0);
15076         final double sy = ma.getElementAt(1, 1);
15077         final double sz = ma.getElementAt(2, 2);
15078         final double mxy = ma.getElementAt(0, 1);
15079         final double mxz = ma.getElementAt(0, 2);
15080         final double myx = ma.getElementAt(1, 0);
15081         final double myz = ma.getElementAt(1, 2);
15082         final double mzx = ma.getElementAt(2, 0);
15083         final double mzy = ma.getElementAt(2, 1);
15084 
15085         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15086         final double latitude = Math.toRadians(
15087                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15088         final double longitude = Math.toRadians(
15089                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15090         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15091         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15092         final NEDVelocity nedVelocity = new NEDVelocity();
15093         final ECEFPosition ecefPosition = new ECEFPosition();
15094         final ECEFVelocity ecefVelocity = new ECEFVelocity();
15095         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15096                 ecefPosition, ecefVelocity);
15097 
15098         final KnownPositionAccelerometerCalibrator calibrator =
15099                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
15100                         true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
15101                         myx, myz, mzx, mzy, this);
15102 
15103         // check default values
15104         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15105         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15106         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15107         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15108         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15109         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15110         final Acceleration bx2 = new Acceleration(0.0,
15111                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15112         calibrator.getInitialBiasXAsAcceleration(bx2);
15113         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15114         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15115         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15116         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15117         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15118         final Acceleration by2 = new Acceleration(0.0,
15119                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15120         calibrator.getInitialBiasYAsAcceleration(by2);
15121         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15122         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15123         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15124         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15125         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15126         final Acceleration bz2 = new Acceleration(0.0,
15127                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15128         calibrator.getInitialBiasZAsAcceleration(bz2);
15129         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15130         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15131         assertEquals(calibrator.getInitialSx(), sx, 0.0);
15132         assertEquals(calibrator.getInitialSy(), sy, 0.0);
15133         assertEquals(calibrator.getInitialSz(), sz, 0.0);
15134         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15135         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15136         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15137         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15138         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15139         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15140         final double[] bias1 = calibrator.getInitialBias();
15141         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
15142         final double[] bias2 = new double[3];
15143         calibrator.getInitialBias(bias2);
15144         assertArrayEquals(bias1, bias2, 0.0);
15145         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15146         assertEquals(b1, ba);
15147         final Matrix b2 = new Matrix(3, 1);
15148         calibrator.getInitialBiasAsMatrix(b2);
15149         assertEquals(b1, b2);
15150         final Matrix ma1 = new Matrix(3, 3);
15151         ma1.setSubmatrix(0, 0,
15152                 2, 2,
15153                 new double[]{sx, myx, mzx,
15154                         mxy, sy, mzy,
15155                         mxz, myz, sz});
15156         assertEquals(calibrator.getInitialMa(), ma1);
15157         final Matrix ma2 = new Matrix(3, 3);
15158         calibrator.getInitialMa(ma2);
15159         assertEquals(ma1, ma2);
15160         assertSame(calibrator.getEcefPosition(), ecefPosition);
15161         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15162         final NEDPosition nedPosition1 = new NEDPosition();
15163         assertTrue(calibrator.getNedPosition(nedPosition1));
15164         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15165         assertSame(calibrator.getMeasurements(), measurements);
15166         assertTrue(calibrator.isCommonAxisUsed());
15167         assertSame(calibrator.getListener(), this);
15168         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
15169         assertFalse(calibrator.isReady());
15170         assertFalse(calibrator.isRunning());
15171         assertNull(calibrator.getEstimatedBiases());
15172         assertFalse(calibrator.getEstimatedBiases(null));
15173         assertNull(calibrator.getEstimatedBiasesAsMatrix());
15174         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15175         assertNull(calibrator.getEstimatedBiasFx());
15176         assertNull(calibrator.getEstimatedBiasFy());
15177         assertNull(calibrator.getEstimatedBiasFz());
15178         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15179         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15180         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15181         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15182         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15183         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15184         assertNull(calibrator.getEstimatedMa());
15185         assertNull(calibrator.getEstimatedSx());
15186         assertNull(calibrator.getEstimatedSy());
15187         assertNull(calibrator.getEstimatedSz());
15188         assertNull(calibrator.getEstimatedMxy());
15189         assertNull(calibrator.getEstimatedMxz());
15190         assertNull(calibrator.getEstimatedMyx());
15191         assertNull(calibrator.getEstimatedMyz());
15192         assertNull(calibrator.getEstimatedMzx());
15193         assertNull(calibrator.getEstimatedMzy());
15194         assertNull(calibrator.getEstimatedCovariance());
15195         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15196         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15197                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15198         assertNotNull(calibrator.getGroundTruthGravityNorm());
15199         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15200         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15201         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15202                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15203         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15204         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15205         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15206     }
15207 
15208     @Test
15209     public void testConstructor125() throws WrongSizeException {
15210         final Matrix ba = generateBa();
15211         final double biasX = ba.getElementAtIndex(0);
15212         final double biasY = ba.getElementAtIndex(1);
15213         final double biasZ = ba.getElementAtIndex(2);
15214 
15215         final Acceleration bx = new Acceleration(biasX,
15216                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15217         final Acceleration by = new Acceleration(biasY,
15218                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15219         final Acceleration bz = new Acceleration(biasZ,
15220                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15221 
15222         final Matrix ma = generateMaCommonAxis();
15223         final double sx = ma.getElementAt(0, 0);
15224         final double sy = ma.getElementAt(1, 1);
15225         final double sz = ma.getElementAt(2, 2);
15226         final double mxy = ma.getElementAt(0, 1);
15227         final double mxz = ma.getElementAt(0, 2);
15228         final double myx = ma.getElementAt(1, 0);
15229         final double myz = ma.getElementAt(1, 2);
15230         final double mzx = ma.getElementAt(2, 0);
15231         final double mzy = ma.getElementAt(2, 1);
15232 
15233         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15234         final double latitude = Math.toRadians(
15235                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15236         final double longitude = Math.toRadians(
15237                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15238         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15239         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15240         final NEDVelocity nedVelocity = new NEDVelocity();
15241         final ECEFPosition ecefPosition = new ECEFPosition();
15242         final ECEFVelocity ecefVelocity = new ECEFVelocity();
15243         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15244                 ecefPosition, ecefVelocity);
15245 
15246         final KnownPositionAccelerometerCalibrator calibrator =
15247                 new KnownPositionAccelerometerCalibrator(ecefPosition,
15248                         bx, by, bz, sx, sy, sz, mxy, mxz,
15249                         myx, myz, mzx, mzy);
15250 
15251         // check default values
15252         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15253         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15254         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15255         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15256         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15257         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15258         final Acceleration bx2 = new Acceleration(0.0,
15259                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15260         calibrator.getInitialBiasXAsAcceleration(bx2);
15261         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15262         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15263         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15264         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15265         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15266         final Acceleration by2 = new Acceleration(0.0,
15267                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15268         calibrator.getInitialBiasYAsAcceleration(by2);
15269         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15270         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15271         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15272         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15273         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15274         final Acceleration bz2 = new Acceleration(0.0,
15275                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15276         calibrator.getInitialBiasZAsAcceleration(bz2);
15277         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15278         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15279         assertEquals(calibrator.getInitialSx(), sx, 0.0);
15280         assertEquals(calibrator.getInitialSy(), sy, 0.0);
15281         assertEquals(calibrator.getInitialSz(), sz, 0.0);
15282         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15283         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15284         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15285         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15286         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15287         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15288         final double[] bias1 = calibrator.getInitialBias();
15289         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
15290         final double[] bias2 = new double[3];
15291         calibrator.getInitialBias(bias2);
15292         assertArrayEquals(bias1, bias2, 0.0);
15293         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15294         assertEquals(b1, ba);
15295         final Matrix b2 = new Matrix(3, 1);
15296         calibrator.getInitialBiasAsMatrix(b2);
15297         assertEquals(b1, b2);
15298         final Matrix ma1 = new Matrix(3, 3);
15299         ma1.setSubmatrix(0, 0,
15300                 2, 2,
15301                 new double[]{sx, myx, mzx,
15302                         mxy, sy, mzy,
15303                         mxz, myz, sz});
15304         assertEquals(calibrator.getInitialMa(), ma1);
15305         final Matrix ma2 = new Matrix(3, 3);
15306         calibrator.getInitialMa(ma2);
15307         assertEquals(ma1, ma2);
15308         assertSame(calibrator.getEcefPosition(), ecefPosition);
15309         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15310         final NEDPosition nedPosition1 = new NEDPosition();
15311         assertTrue(calibrator.getNedPosition(nedPosition1));
15312         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15313         assertNull(calibrator.getMeasurements());
15314         assertFalse(calibrator.isCommonAxisUsed());
15315         assertNull(calibrator.getListener());
15316         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
15317         assertFalse(calibrator.isReady());
15318         assertFalse(calibrator.isRunning());
15319         assertNull(calibrator.getEstimatedBiases());
15320         assertFalse(calibrator.getEstimatedBiases(null));
15321         assertNull(calibrator.getEstimatedBiasesAsMatrix());
15322         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15323         assertNull(calibrator.getEstimatedBiasFx());
15324         assertNull(calibrator.getEstimatedBiasFy());
15325         assertNull(calibrator.getEstimatedBiasFz());
15326         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15327         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15328         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15329         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15330         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15331         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15332         assertNull(calibrator.getEstimatedMa());
15333         assertNull(calibrator.getEstimatedSx());
15334         assertNull(calibrator.getEstimatedSy());
15335         assertNull(calibrator.getEstimatedSz());
15336         assertNull(calibrator.getEstimatedMxy());
15337         assertNull(calibrator.getEstimatedMxz());
15338         assertNull(calibrator.getEstimatedMyx());
15339         assertNull(calibrator.getEstimatedMyz());
15340         assertNull(calibrator.getEstimatedMzx());
15341         assertNull(calibrator.getEstimatedMzy());
15342         assertNull(calibrator.getEstimatedCovariance());
15343         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15344         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15345                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15346         assertNotNull(calibrator.getGroundTruthGravityNorm());
15347         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15348         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15349         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15350                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15351         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15352         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15353         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15354     }
15355 
15356     @Test
15357     public void testConstructor126() throws WrongSizeException {
15358         final Matrix ba = generateBa();
15359         final double biasX = ba.getElementAtIndex(0);
15360         final double biasY = ba.getElementAtIndex(1);
15361         final double biasZ = ba.getElementAtIndex(2);
15362 
15363         final Acceleration bx = new Acceleration(biasX,
15364                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15365         final Acceleration by = new Acceleration(biasY,
15366                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15367         final Acceleration bz = new Acceleration(biasZ,
15368                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15369 
15370         final Matrix ma = generateMaCommonAxis();
15371         final double sx = ma.getElementAt(0, 0);
15372         final double sy = ma.getElementAt(1, 1);
15373         final double sz = ma.getElementAt(2, 2);
15374         final double mxy = ma.getElementAt(0, 1);
15375         final double mxz = ma.getElementAt(0, 2);
15376         final double myx = ma.getElementAt(1, 0);
15377         final double myz = ma.getElementAt(1, 2);
15378         final double mzx = ma.getElementAt(2, 0);
15379         final double mzy = ma.getElementAt(2, 1);
15380 
15381         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15382         final double latitude = Math.toRadians(
15383                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15384         final double longitude = Math.toRadians(
15385                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15386         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15387         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15388         final NEDVelocity nedVelocity = new NEDVelocity();
15389         final ECEFPosition ecefPosition = new ECEFPosition();
15390         final ECEFVelocity ecefVelocity = new ECEFVelocity();
15391         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15392                 ecefPosition, ecefVelocity);
15393 
15394         final KnownPositionAccelerometerCalibrator calibrator =
15395                 new KnownPositionAccelerometerCalibrator(ecefPosition,
15396                         bx, by, bz, sx, sy, sz, mxy, mxz,
15397                         myx, myz, mzx, mzy, this);
15398 
15399         // check default values
15400         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15401         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15402         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15403         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15404         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15405         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15406         final Acceleration bx2 = new Acceleration(0.0,
15407                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15408         calibrator.getInitialBiasXAsAcceleration(bx2);
15409         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15410         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15411         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15412         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15413         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15414         final Acceleration by2 = new Acceleration(0.0,
15415                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15416         calibrator.getInitialBiasYAsAcceleration(by2);
15417         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15418         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15419         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15420         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15421         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15422         final Acceleration bz2 = new Acceleration(0.0,
15423                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15424         calibrator.getInitialBiasZAsAcceleration(bz2);
15425         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15426         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15427         assertEquals(calibrator.getInitialSx(), sx, 0.0);
15428         assertEquals(calibrator.getInitialSy(), sy, 0.0);
15429         assertEquals(calibrator.getInitialSz(), sz, 0.0);
15430         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15431         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15432         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15433         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15434         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15435         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15436         final double[] bias1 = calibrator.getInitialBias();
15437         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
15438         final double[] bias2 = new double[3];
15439         calibrator.getInitialBias(bias2);
15440         assertArrayEquals(bias1, bias2, 0.0);
15441         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15442         assertEquals(b1, ba);
15443         final Matrix b2 = new Matrix(3, 1);
15444         calibrator.getInitialBiasAsMatrix(b2);
15445         assertEquals(b1, b2);
15446         final Matrix ma1 = new Matrix(3, 3);
15447         ma1.setSubmatrix(0, 0,
15448                 2, 2,
15449                 new double[]{sx, myx, mzx,
15450                         mxy, sy, mzy,
15451                         mxz, myz, sz});
15452         assertEquals(calibrator.getInitialMa(), ma1);
15453         final Matrix ma2 = new Matrix(3, 3);
15454         calibrator.getInitialMa(ma2);
15455         assertEquals(ma1, ma2);
15456         assertSame(calibrator.getEcefPosition(), ecefPosition);
15457         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15458         final NEDPosition nedPosition1 = new NEDPosition();
15459         assertTrue(calibrator.getNedPosition(nedPosition1));
15460         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15461         assertNull(calibrator.getMeasurements());
15462         assertFalse(calibrator.isCommonAxisUsed());
15463         assertSame(calibrator.getListener(), this);
15464         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
15465         assertFalse(calibrator.isReady());
15466         assertFalse(calibrator.isRunning());
15467         assertNull(calibrator.getEstimatedBiases());
15468         assertFalse(calibrator.getEstimatedBiases(null));
15469         assertNull(calibrator.getEstimatedBiasesAsMatrix());
15470         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15471         assertNull(calibrator.getEstimatedBiasFx());
15472         assertNull(calibrator.getEstimatedBiasFy());
15473         assertNull(calibrator.getEstimatedBiasFz());
15474         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15475         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15476         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15477         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15478         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15479         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15480         assertNull(calibrator.getEstimatedMa());
15481         assertNull(calibrator.getEstimatedSx());
15482         assertNull(calibrator.getEstimatedSy());
15483         assertNull(calibrator.getEstimatedSz());
15484         assertNull(calibrator.getEstimatedMxy());
15485         assertNull(calibrator.getEstimatedMxz());
15486         assertNull(calibrator.getEstimatedMyx());
15487         assertNull(calibrator.getEstimatedMyz());
15488         assertNull(calibrator.getEstimatedMzx());
15489         assertNull(calibrator.getEstimatedMzy());
15490         assertNull(calibrator.getEstimatedCovariance());
15491         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15492         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15493                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15494         assertNotNull(calibrator.getGroundTruthGravityNorm());
15495         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15496         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15497         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15498                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15499         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15500         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15501         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15502     }
15503 
15504     @Test
15505     public void testConstructor127() throws WrongSizeException {
15506         final Collection<StandardDeviationBodyKinematics> measurements =
15507                 Collections.emptyList();
15508 
15509         final Matrix ba = generateBa();
15510         final double biasX = ba.getElementAtIndex(0);
15511         final double biasY = ba.getElementAtIndex(1);
15512         final double biasZ = ba.getElementAtIndex(2);
15513 
15514         final Acceleration bx = new Acceleration(biasX,
15515                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15516         final Acceleration by = new Acceleration(biasY,
15517                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15518         final Acceleration bz = new Acceleration(biasZ,
15519                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15520 
15521         final Matrix ma = generateMaCommonAxis();
15522         final double sx = ma.getElementAt(0, 0);
15523         final double sy = ma.getElementAt(1, 1);
15524         final double sz = ma.getElementAt(2, 2);
15525         final double mxy = ma.getElementAt(0, 1);
15526         final double mxz = ma.getElementAt(0, 2);
15527         final double myx = ma.getElementAt(1, 0);
15528         final double myz = ma.getElementAt(1, 2);
15529         final double mzx = ma.getElementAt(2, 0);
15530         final double mzy = ma.getElementAt(2, 1);
15531 
15532         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15533         final double latitude = Math.toRadians(
15534                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15535         final double longitude = Math.toRadians(
15536                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15537         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15538         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15539         final NEDVelocity nedVelocity = new NEDVelocity();
15540         final ECEFPosition ecefPosition = new ECEFPosition();
15541         final ECEFVelocity ecefVelocity = new ECEFVelocity();
15542         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15543                 ecefPosition, ecefVelocity);
15544 
15545         final KnownPositionAccelerometerCalibrator calibrator =
15546                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
15547                         bx, by, bz, sx, sy, sz, mxy, mxz,
15548                         myx, myz, mzx, mzy);
15549 
15550         // check default values
15551         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15552         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15553         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15554         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15555         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15556         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15557         final Acceleration bx2 = new Acceleration(0.0,
15558                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15559         calibrator.getInitialBiasXAsAcceleration(bx2);
15560         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15561         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15562         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15563         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15564         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15565         final Acceleration by2 = new Acceleration(0.0,
15566                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15567         calibrator.getInitialBiasYAsAcceleration(by2);
15568         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15569         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15570         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15571         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15572         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15573         final Acceleration bz2 = new Acceleration(0.0,
15574                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15575         calibrator.getInitialBiasZAsAcceleration(bz2);
15576         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15577         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15578         assertEquals(calibrator.getInitialSx(), sx, 0.0);
15579         assertEquals(calibrator.getInitialSy(), sy, 0.0);
15580         assertEquals(calibrator.getInitialSz(), sz, 0.0);
15581         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15582         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15583         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15584         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15585         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15586         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15587         final double[] bias1 = calibrator.getInitialBias();
15588         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
15589         final double[] bias2 = new double[3];
15590         calibrator.getInitialBias(bias2);
15591         assertArrayEquals(bias1, bias2, 0.0);
15592         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15593         assertEquals(b1, ba);
15594         final Matrix b2 = new Matrix(3, 1);
15595         calibrator.getInitialBiasAsMatrix(b2);
15596         assertEquals(b1, b2);
15597         final Matrix ma1 = new Matrix(3, 3);
15598         ma1.setSubmatrix(0, 0,
15599                 2, 2,
15600                 new double[]{sx, myx, mzx,
15601                         mxy, sy, mzy,
15602                         mxz, myz, sz});
15603         assertEquals(calibrator.getInitialMa(), ma1);
15604         final Matrix ma2 = new Matrix(3, 3);
15605         calibrator.getInitialMa(ma2);
15606         assertEquals(ma1, ma2);
15607         assertSame(calibrator.getEcefPosition(), ecefPosition);
15608         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15609         final NEDPosition nedPosition1 = new NEDPosition();
15610         assertTrue(calibrator.getNedPosition(nedPosition1));
15611         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15612         assertSame(calibrator.getMeasurements(), measurements);
15613         assertFalse(calibrator.isCommonAxisUsed());
15614         assertNull(calibrator.getListener());
15615         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
15616         assertFalse(calibrator.isReady());
15617         assertFalse(calibrator.isRunning());
15618         assertNull(calibrator.getEstimatedBiases());
15619         assertFalse(calibrator.getEstimatedBiases(null));
15620         assertNull(calibrator.getEstimatedBiasesAsMatrix());
15621         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15622         assertNull(calibrator.getEstimatedBiasFx());
15623         assertNull(calibrator.getEstimatedBiasFy());
15624         assertNull(calibrator.getEstimatedBiasFz());
15625         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15626         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15627         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15628         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15629         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15630         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15631         assertNull(calibrator.getEstimatedMa());
15632         assertNull(calibrator.getEstimatedSx());
15633         assertNull(calibrator.getEstimatedSy());
15634         assertNull(calibrator.getEstimatedSz());
15635         assertNull(calibrator.getEstimatedMxy());
15636         assertNull(calibrator.getEstimatedMxz());
15637         assertNull(calibrator.getEstimatedMyx());
15638         assertNull(calibrator.getEstimatedMyz());
15639         assertNull(calibrator.getEstimatedMzx());
15640         assertNull(calibrator.getEstimatedMzy());
15641         assertNull(calibrator.getEstimatedCovariance());
15642         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15643         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15644                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15645         assertNotNull(calibrator.getGroundTruthGravityNorm());
15646         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15647         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15648         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15649                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15650         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15651         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15652         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15653     }
15654 
15655     @Test
15656     public void testConstructor128() throws WrongSizeException {
15657         final Collection<StandardDeviationBodyKinematics> measurements =
15658                 Collections.emptyList();
15659 
15660         final Matrix ba = generateBa();
15661         final double biasX = ba.getElementAtIndex(0);
15662         final double biasY = ba.getElementAtIndex(1);
15663         final double biasZ = ba.getElementAtIndex(2);
15664 
15665         final Acceleration bx = new Acceleration(biasX,
15666                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15667         final Acceleration by = new Acceleration(biasY,
15668                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15669         final Acceleration bz = new Acceleration(biasZ,
15670                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15671 
15672         final Matrix ma = generateMaCommonAxis();
15673         final double sx = ma.getElementAt(0, 0);
15674         final double sy = ma.getElementAt(1, 1);
15675         final double sz = ma.getElementAt(2, 2);
15676         final double mxy = ma.getElementAt(0, 1);
15677         final double mxz = ma.getElementAt(0, 2);
15678         final double myx = ma.getElementAt(1, 0);
15679         final double myz = ma.getElementAt(1, 2);
15680         final double mzx = ma.getElementAt(2, 0);
15681         final double mzy = ma.getElementAt(2, 1);
15682 
15683         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15684         final double latitude = Math.toRadians(
15685                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15686         final double longitude = Math.toRadians(
15687                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15688         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15689         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15690         final NEDVelocity nedVelocity = new NEDVelocity();
15691         final ECEFPosition ecefPosition = new ECEFPosition();
15692         final ECEFVelocity ecefVelocity = new ECEFVelocity();
15693         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15694                 ecefPosition, ecefVelocity);
15695 
15696         final KnownPositionAccelerometerCalibrator calibrator =
15697                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
15698                         bx, by, bz, sx, sy, sz, mxy, mxz,
15699                         myx, myz, mzx, mzy, this);
15700 
15701         // check default values
15702         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15703         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15704         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15705         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15706         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15707         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15708         final Acceleration bx2 = new Acceleration(0.0,
15709                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15710         calibrator.getInitialBiasXAsAcceleration(bx2);
15711         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15712         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15713         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15714         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15715         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15716         final Acceleration by2 = new Acceleration(0.0,
15717                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15718         calibrator.getInitialBiasYAsAcceleration(by2);
15719         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15720         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15721         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15722         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15723         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15724         final Acceleration bz2 = new Acceleration(0.0,
15725                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15726         calibrator.getInitialBiasZAsAcceleration(bz2);
15727         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15728         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15729         assertEquals(calibrator.getInitialSx(), sx, 0.0);
15730         assertEquals(calibrator.getInitialSy(), sy, 0.0);
15731         assertEquals(calibrator.getInitialSz(), sz, 0.0);
15732         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15733         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15734         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15735         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15736         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15737         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15738         final double[] bias1 = calibrator.getInitialBias();
15739         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
15740         final double[] bias2 = new double[3];
15741         calibrator.getInitialBias(bias2);
15742         assertArrayEquals(bias1, bias2, 0.0);
15743         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15744         assertEquals(b1, ba);
15745         final Matrix b2 = new Matrix(3, 1);
15746         calibrator.getInitialBiasAsMatrix(b2);
15747         assertEquals(b1, b2);
15748         final Matrix ma1 = new Matrix(3, 3);
15749         ma1.setSubmatrix(0, 0,
15750                 2, 2,
15751                 new double[]{sx, myx, mzx,
15752                         mxy, sy, mzy,
15753                         mxz, myz, sz});
15754         assertEquals(calibrator.getInitialMa(), ma1);
15755         final Matrix ma2 = new Matrix(3, 3);
15756         calibrator.getInitialMa(ma2);
15757         assertEquals(ma1, ma2);
15758         assertSame(calibrator.getEcefPosition(), ecefPosition);
15759         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15760         final NEDPosition nedPosition1 = new NEDPosition();
15761         assertTrue(calibrator.getNedPosition(nedPosition1));
15762         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15763         assertSame(calibrator.getMeasurements(), measurements);
15764         assertFalse(calibrator.isCommonAxisUsed());
15765         assertSame(calibrator.getListener(), this);
15766         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
15767         assertFalse(calibrator.isReady());
15768         assertFalse(calibrator.isRunning());
15769         assertNull(calibrator.getEstimatedBiases());
15770         assertFalse(calibrator.getEstimatedBiases(null));
15771         assertNull(calibrator.getEstimatedBiasesAsMatrix());
15772         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15773         assertNull(calibrator.getEstimatedBiasFx());
15774         assertNull(calibrator.getEstimatedBiasFy());
15775         assertNull(calibrator.getEstimatedBiasFz());
15776         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15777         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15778         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15779         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15780         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15781         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15782         assertNull(calibrator.getEstimatedMa());
15783         assertNull(calibrator.getEstimatedSx());
15784         assertNull(calibrator.getEstimatedSy());
15785         assertNull(calibrator.getEstimatedSz());
15786         assertNull(calibrator.getEstimatedMxy());
15787         assertNull(calibrator.getEstimatedMxz());
15788         assertNull(calibrator.getEstimatedMyx());
15789         assertNull(calibrator.getEstimatedMyz());
15790         assertNull(calibrator.getEstimatedMzx());
15791         assertNull(calibrator.getEstimatedMzy());
15792         assertNull(calibrator.getEstimatedCovariance());
15793         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15794         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15795                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15796         assertNotNull(calibrator.getGroundTruthGravityNorm());
15797         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15798         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15799         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15800                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15801         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15802         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15803         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15804     }
15805 
15806     @Test
15807     public void testConstructor129() throws WrongSizeException {
15808         final Matrix ba = generateBa();
15809         final double biasX = ba.getElementAtIndex(0);
15810         final double biasY = ba.getElementAtIndex(1);
15811         final double biasZ = ba.getElementAtIndex(2);
15812 
15813         final Acceleration bx = new Acceleration(biasX,
15814                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15815         final Acceleration by = new Acceleration(biasY,
15816                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15817         final Acceleration bz = new Acceleration(biasZ,
15818                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15819 
15820         final Matrix ma = generateMaCommonAxis();
15821         final double sx = ma.getElementAt(0, 0);
15822         final double sy = ma.getElementAt(1, 1);
15823         final double sz = ma.getElementAt(2, 2);
15824         final double mxy = ma.getElementAt(0, 1);
15825         final double mxz = ma.getElementAt(0, 2);
15826         final double myx = ma.getElementAt(1, 0);
15827         final double myz = ma.getElementAt(1, 2);
15828         final double mzx = ma.getElementAt(2, 0);
15829         final double mzy = ma.getElementAt(2, 1);
15830 
15831         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15832         final double latitude = Math.toRadians(
15833                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15834         final double longitude = Math.toRadians(
15835                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15836         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15837         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15838         final NEDVelocity nedVelocity = new NEDVelocity();
15839         final ECEFPosition ecefPosition = new ECEFPosition();
15840         final ECEFVelocity ecefVelocity = new ECEFVelocity();
15841         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15842                 ecefPosition, ecefVelocity);
15843 
15844         final KnownPositionAccelerometerCalibrator calibrator =
15845                 new KnownPositionAccelerometerCalibrator(ecefPosition,
15846                         true, bx, by, bz, sx, sy, sz, mxy, mxz,
15847                         myx, myz, mzx, mzy);
15848 
15849         // check default values
15850         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15851         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15852         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15853         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15854         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15855         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15856         final Acceleration bx2 = new Acceleration(0.0,
15857                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15858         calibrator.getInitialBiasXAsAcceleration(bx2);
15859         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15860         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15861         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15862         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15863         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15864         final Acceleration by2 = new Acceleration(0.0,
15865                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15866         calibrator.getInitialBiasYAsAcceleration(by2);
15867         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15868         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15869         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15870         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15871         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15872         final Acceleration bz2 = new Acceleration(0.0,
15873                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15874         calibrator.getInitialBiasZAsAcceleration(bz2);
15875         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15876         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15877         assertEquals(calibrator.getInitialSx(), sx, 0.0);
15878         assertEquals(calibrator.getInitialSy(), sy, 0.0);
15879         assertEquals(calibrator.getInitialSz(), sz, 0.0);
15880         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15881         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15882         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15883         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15884         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15885         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15886         final double[] bias1 = calibrator.getInitialBias();
15887         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
15888         final double[] bias2 = new double[3];
15889         calibrator.getInitialBias(bias2);
15890         assertArrayEquals(bias1, bias2, 0.0);
15891         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15892         assertEquals(b1, ba);
15893         final Matrix b2 = new Matrix(3, 1);
15894         calibrator.getInitialBiasAsMatrix(b2);
15895         assertEquals(b1, b2);
15896         final Matrix ma1 = new Matrix(3, 3);
15897         ma1.setSubmatrix(0, 0,
15898                 2, 2,
15899                 new double[]{sx, myx, mzx,
15900                         mxy, sy, mzy,
15901                         mxz, myz, sz});
15902         assertEquals(calibrator.getInitialMa(), ma1);
15903         final Matrix ma2 = new Matrix(3, 3);
15904         calibrator.getInitialMa(ma2);
15905         assertEquals(ma1, ma2);
15906         assertSame(calibrator.getEcefPosition(), ecefPosition);
15907         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15908         final NEDPosition nedPosition1 = new NEDPosition();
15909         assertTrue(calibrator.getNedPosition(nedPosition1));
15910         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15911         assertNull(calibrator.getMeasurements());
15912         assertTrue(calibrator.isCommonAxisUsed());
15913         assertNull(calibrator.getListener());
15914         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
15915         assertFalse(calibrator.isReady());
15916         assertFalse(calibrator.isRunning());
15917         assertNull(calibrator.getEstimatedBiases());
15918         assertFalse(calibrator.getEstimatedBiases(null));
15919         assertNull(calibrator.getEstimatedBiasesAsMatrix());
15920         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15921         assertNull(calibrator.getEstimatedBiasFx());
15922         assertNull(calibrator.getEstimatedBiasFy());
15923         assertNull(calibrator.getEstimatedBiasFz());
15924         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15925         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15926         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15927         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15928         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15929         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15930         assertNull(calibrator.getEstimatedMa());
15931         assertNull(calibrator.getEstimatedSx());
15932         assertNull(calibrator.getEstimatedSy());
15933         assertNull(calibrator.getEstimatedSz());
15934         assertNull(calibrator.getEstimatedMxy());
15935         assertNull(calibrator.getEstimatedMxz());
15936         assertNull(calibrator.getEstimatedMyx());
15937         assertNull(calibrator.getEstimatedMyz());
15938         assertNull(calibrator.getEstimatedMzx());
15939         assertNull(calibrator.getEstimatedMzy());
15940         assertNull(calibrator.getEstimatedCovariance());
15941         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15942         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15943                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15944         assertNotNull(calibrator.getGroundTruthGravityNorm());
15945         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15946         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15947         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15948                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15949         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15950         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15951         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15952     }
15953 
15954     @Test
15955     public void testConstructor130() throws WrongSizeException {
15956         final Matrix ba = generateBa();
15957         final double biasX = ba.getElementAtIndex(0);
15958         final double biasY = ba.getElementAtIndex(1);
15959         final double biasZ = ba.getElementAtIndex(2);
15960 
15961         final Acceleration bx = new Acceleration(biasX,
15962                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15963         final Acceleration by = new Acceleration(biasY,
15964                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15965         final Acceleration bz = new Acceleration(biasZ,
15966                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15967 
15968         final Matrix ma = generateMaCommonAxis();
15969         final double sx = ma.getElementAt(0, 0);
15970         final double sy = ma.getElementAt(1, 1);
15971         final double sz = ma.getElementAt(2, 2);
15972         final double mxy = ma.getElementAt(0, 1);
15973         final double mxz = ma.getElementAt(0, 2);
15974         final double myx = ma.getElementAt(1, 0);
15975         final double myz = ma.getElementAt(1, 2);
15976         final double mzx = ma.getElementAt(2, 0);
15977         final double mzy = ma.getElementAt(2, 1);
15978 
15979         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15980         final double latitude = Math.toRadians(
15981                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15982         final double longitude = Math.toRadians(
15983                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15984         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15985         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15986         final NEDVelocity nedVelocity = new NEDVelocity();
15987         final ECEFPosition ecefPosition = new ECEFPosition();
15988         final ECEFVelocity ecefVelocity = new ECEFVelocity();
15989         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15990                 ecefPosition, ecefVelocity);
15991 
15992         final KnownPositionAccelerometerCalibrator calibrator =
15993                 new KnownPositionAccelerometerCalibrator(ecefPosition,
15994                         true, bx, by, bz, sx, sy, sz, mxy, mxz,
15995                         myx, myz, mzx, mzy, this);
15996 
15997         // check default values
15998         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15999         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16000         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16001         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16002         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16003         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16004         final Acceleration bx2 = new Acceleration(0.0,
16005                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16006         calibrator.getInitialBiasXAsAcceleration(bx2);
16007         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16008         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16009         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16010         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16011         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16012         final Acceleration by2 = new Acceleration(0.0,
16013                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16014         calibrator.getInitialBiasYAsAcceleration(by2);
16015         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16016         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16017         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16018         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16019         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16020         final Acceleration bz2 = new Acceleration(0.0,
16021                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16022         calibrator.getInitialBiasZAsAcceleration(bz2);
16023         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16024         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16025         assertEquals(calibrator.getInitialSx(), sx, 0.0);
16026         assertEquals(calibrator.getInitialSy(), sy, 0.0);
16027         assertEquals(calibrator.getInitialSz(), sz, 0.0);
16028         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
16029         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
16030         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
16031         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
16032         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
16033         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
16034         final double[] bias1 = calibrator.getInitialBias();
16035         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
16036         final double[] bias2 = new double[3];
16037         calibrator.getInitialBias(bias2);
16038         assertArrayEquals(bias1, bias2, 0.0);
16039         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16040         assertEquals(b1, ba);
16041         final Matrix b2 = new Matrix(3, 1);
16042         calibrator.getInitialBiasAsMatrix(b2);
16043         assertEquals(b1, b2);
16044         final Matrix ma1 = new Matrix(3, 3);
16045         ma1.setSubmatrix(0, 0,
16046                 2, 2,
16047                 new double[]{sx, myx, mzx,
16048                         mxy, sy, mzy,
16049                         mxz, myz, sz});
16050         assertEquals(calibrator.getInitialMa(), ma1);
16051         final Matrix ma2 = new Matrix(3, 3);
16052         calibrator.getInitialMa(ma2);
16053         assertEquals(ma1, ma2);
16054         assertSame(calibrator.getEcefPosition(), ecefPosition);
16055         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16056         final NEDPosition nedPosition1 = new NEDPosition();
16057         assertTrue(calibrator.getNedPosition(nedPosition1));
16058         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16059         assertNull(calibrator.getMeasurements());
16060         assertTrue(calibrator.isCommonAxisUsed());
16061         assertSame(calibrator.getListener(), this);
16062         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
16063         assertFalse(calibrator.isReady());
16064         assertFalse(calibrator.isRunning());
16065         assertNull(calibrator.getEstimatedBiases());
16066         assertFalse(calibrator.getEstimatedBiases(null));
16067         assertNull(calibrator.getEstimatedBiasesAsMatrix());
16068         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16069         assertNull(calibrator.getEstimatedBiasFx());
16070         assertNull(calibrator.getEstimatedBiasFy());
16071         assertNull(calibrator.getEstimatedBiasFz());
16072         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16073         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16074         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16075         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16076         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16077         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16078         assertNull(calibrator.getEstimatedMa());
16079         assertNull(calibrator.getEstimatedSx());
16080         assertNull(calibrator.getEstimatedSy());
16081         assertNull(calibrator.getEstimatedSz());
16082         assertNull(calibrator.getEstimatedMxy());
16083         assertNull(calibrator.getEstimatedMxz());
16084         assertNull(calibrator.getEstimatedMyx());
16085         assertNull(calibrator.getEstimatedMyz());
16086         assertNull(calibrator.getEstimatedMzx());
16087         assertNull(calibrator.getEstimatedMzy());
16088         assertNull(calibrator.getEstimatedCovariance());
16089         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16090         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16091                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16092         assertNotNull(calibrator.getGroundTruthGravityNorm());
16093         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16094         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16095         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16096                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16097         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16098         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16099         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16100     }
16101 
16102     @Test
16103     public void testConstructor131() throws WrongSizeException {
16104         final Collection<StandardDeviationBodyKinematics> measurements =
16105                 Collections.emptyList();
16106 
16107         final Matrix ba = generateBa();
16108         final double biasX = ba.getElementAtIndex(0);
16109         final double biasY = ba.getElementAtIndex(1);
16110         final double biasZ = ba.getElementAtIndex(2);
16111 
16112         final Acceleration bx = new Acceleration(biasX,
16113                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16114         final Acceleration by = new Acceleration(biasY,
16115                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16116         final Acceleration bz = new Acceleration(biasZ,
16117                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16118 
16119         final Matrix ma = generateMaCommonAxis();
16120         final double sx = ma.getElementAt(0, 0);
16121         final double sy = ma.getElementAt(1, 1);
16122         final double sz = ma.getElementAt(2, 2);
16123         final double mxy = ma.getElementAt(0, 1);
16124         final double mxz = ma.getElementAt(0, 2);
16125         final double myx = ma.getElementAt(1, 0);
16126         final double myz = ma.getElementAt(1, 2);
16127         final double mzx = ma.getElementAt(2, 0);
16128         final double mzy = ma.getElementAt(2, 1);
16129 
16130         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16131         final double latitude = Math.toRadians(
16132                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16133         final double longitude = Math.toRadians(
16134                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16135         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16136         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16137         final NEDVelocity nedVelocity = new NEDVelocity();
16138         final ECEFPosition ecefPosition = new ECEFPosition();
16139         final ECEFVelocity ecefVelocity = new ECEFVelocity();
16140         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16141                 ecefPosition, ecefVelocity);
16142 
16143         final KnownPositionAccelerometerCalibrator calibrator =
16144                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
16145                         true, bx, by, bz, sx, sy, sz, mxy, mxz,
16146                         myx, myz, mzx, mzy);
16147 
16148         // check default values
16149         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16150         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16151         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16152         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16153         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16154         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16155         final Acceleration bx2 = new Acceleration(0.0,
16156                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16157         calibrator.getInitialBiasXAsAcceleration(bx2);
16158         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16159         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16160         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16161         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16162         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16163         final Acceleration by2 = new Acceleration(0.0,
16164                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16165         calibrator.getInitialBiasYAsAcceleration(by2);
16166         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16167         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16168         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16169         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16170         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16171         final Acceleration bz2 = new Acceleration(0.0,
16172                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16173         calibrator.getInitialBiasZAsAcceleration(bz2);
16174         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16175         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16176         assertEquals(calibrator.getInitialSx(), sx, 0.0);
16177         assertEquals(calibrator.getInitialSy(), sy, 0.0);
16178         assertEquals(calibrator.getInitialSz(), sz, 0.0);
16179         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
16180         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
16181         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
16182         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
16183         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
16184         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
16185         final double[] bias1 = calibrator.getInitialBias();
16186         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
16187         final double[] bias2 = new double[3];
16188         calibrator.getInitialBias(bias2);
16189         assertArrayEquals(bias1, bias2, 0.0);
16190         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16191         assertEquals(b1, ba);
16192         final Matrix b2 = new Matrix(3, 1);
16193         calibrator.getInitialBiasAsMatrix(b2);
16194         assertEquals(b1, b2);
16195         final Matrix ma1 = new Matrix(3, 3);
16196         ma1.setSubmatrix(0, 0,
16197                 2, 2,
16198                 new double[]{sx, myx, mzx,
16199                         mxy, sy, mzy,
16200                         mxz, myz, sz});
16201         assertEquals(calibrator.getInitialMa(), ma1);
16202         final Matrix ma2 = new Matrix(3, 3);
16203         calibrator.getInitialMa(ma2);
16204         assertEquals(ma1, ma2);
16205         assertSame(calibrator.getEcefPosition(), ecefPosition);
16206         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16207         final NEDPosition nedPosition1 = new NEDPosition();
16208         assertTrue(calibrator.getNedPosition(nedPosition1));
16209         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16210         assertSame(calibrator.getMeasurements(), measurements);
16211         assertTrue(calibrator.isCommonAxisUsed());
16212         assertNull(calibrator.getListener());
16213         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
16214         assertFalse(calibrator.isReady());
16215         assertFalse(calibrator.isRunning());
16216         assertNull(calibrator.getEstimatedBiases());
16217         assertFalse(calibrator.getEstimatedBiases(null));
16218         assertNull(calibrator.getEstimatedBiasesAsMatrix());
16219         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16220         assertNull(calibrator.getEstimatedBiasFx());
16221         assertNull(calibrator.getEstimatedBiasFy());
16222         assertNull(calibrator.getEstimatedBiasFz());
16223         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16224         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16225         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16226         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16227         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16228         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16229         assertNull(calibrator.getEstimatedMa());
16230         assertNull(calibrator.getEstimatedSx());
16231         assertNull(calibrator.getEstimatedSy());
16232         assertNull(calibrator.getEstimatedSz());
16233         assertNull(calibrator.getEstimatedMxy());
16234         assertNull(calibrator.getEstimatedMxz());
16235         assertNull(calibrator.getEstimatedMyx());
16236         assertNull(calibrator.getEstimatedMyz());
16237         assertNull(calibrator.getEstimatedMzx());
16238         assertNull(calibrator.getEstimatedMzy());
16239         assertNull(calibrator.getEstimatedCovariance());
16240         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16241         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16242                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16243         assertNotNull(calibrator.getGroundTruthGravityNorm());
16244         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16245         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16246         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16247                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16248         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16249         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16250         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16251     }
16252 
16253     @Test
16254     public void testConstructor132() throws WrongSizeException {
16255         final Collection<StandardDeviationBodyKinematics> measurements =
16256                 Collections.emptyList();
16257 
16258         final Matrix ba = generateBa();
16259         final double biasX = ba.getElementAtIndex(0);
16260         final double biasY = ba.getElementAtIndex(1);
16261         final double biasZ = ba.getElementAtIndex(2);
16262 
16263         final Acceleration bx = new Acceleration(biasX,
16264                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16265         final Acceleration by = new Acceleration(biasY,
16266                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16267         final Acceleration bz = new Acceleration(biasZ,
16268                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16269 
16270         final Matrix ma = generateMaCommonAxis();
16271         final double sx = ma.getElementAt(0, 0);
16272         final double sy = ma.getElementAt(1, 1);
16273         final double sz = ma.getElementAt(2, 2);
16274         final double mxy = ma.getElementAt(0, 1);
16275         final double mxz = ma.getElementAt(0, 2);
16276         final double myx = ma.getElementAt(1, 0);
16277         final double myz = ma.getElementAt(1, 2);
16278         final double mzx = ma.getElementAt(2, 0);
16279         final double mzy = ma.getElementAt(2, 1);
16280 
16281         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16282         final double latitude = Math.toRadians(
16283                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16284         final double longitude = Math.toRadians(
16285                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16286         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16287         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16288         final NEDVelocity nedVelocity = new NEDVelocity();
16289         final ECEFPosition ecefPosition = new ECEFPosition();
16290         final ECEFVelocity ecefVelocity = new ECEFVelocity();
16291         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16292                 ecefPosition, ecefVelocity);
16293 
16294         final KnownPositionAccelerometerCalibrator calibrator =
16295                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
16296                         true, bx, by, bz, sx, sy, sz, mxy, mxz,
16297                         myx, myz, mzx, mzy, this);
16298 
16299         // check default values
16300         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16301         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16302         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16303         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16304         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16305         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16306         final Acceleration bx2 = new Acceleration(0.0,
16307                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16308         calibrator.getInitialBiasXAsAcceleration(bx2);
16309         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16310         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16311         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16312         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16313         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16314         final Acceleration by2 = new Acceleration(0.0,
16315                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16316         calibrator.getInitialBiasYAsAcceleration(by2);
16317         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16318         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16319         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16320         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16321         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16322         final Acceleration bz2 = new Acceleration(0.0,
16323                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16324         calibrator.getInitialBiasZAsAcceleration(bz2);
16325         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16326         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16327         assertEquals(calibrator.getInitialSx(), sx, 0.0);
16328         assertEquals(calibrator.getInitialSy(), sy, 0.0);
16329         assertEquals(calibrator.getInitialSz(), sz, 0.0);
16330         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
16331         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
16332         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
16333         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
16334         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
16335         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
16336         final double[] bias1 = calibrator.getInitialBias();
16337         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
16338         final double[] bias2 = new double[3];
16339         calibrator.getInitialBias(bias2);
16340         assertArrayEquals(bias1, bias2, 0.0);
16341         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16342         assertEquals(b1, ba);
16343         final Matrix b2 = new Matrix(3, 1);
16344         calibrator.getInitialBiasAsMatrix(b2);
16345         assertEquals(b1, b2);
16346         final Matrix ma1 = new Matrix(3, 3);
16347         ma1.setSubmatrix(0, 0,
16348                 2, 2,
16349                 new double[]{sx, myx, mzx,
16350                         mxy, sy, mzy,
16351                         mxz, myz, sz});
16352         assertEquals(calibrator.getInitialMa(), ma1);
16353         final Matrix ma2 = new Matrix(3, 3);
16354         calibrator.getInitialMa(ma2);
16355         assertEquals(ma1, ma2);
16356         assertSame(calibrator.getEcefPosition(), ecefPosition);
16357         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16358         final NEDPosition nedPosition1 = new NEDPosition();
16359         assertTrue(calibrator.getNedPosition(nedPosition1));
16360         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16361         assertSame(calibrator.getMeasurements(), measurements);
16362         assertTrue(calibrator.isCommonAxisUsed());
16363         assertSame(calibrator.getListener(), this);
16364         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
16365         assertFalse(calibrator.isReady());
16366         assertFalse(calibrator.isRunning());
16367         assertNull(calibrator.getEstimatedBiases());
16368         assertFalse(calibrator.getEstimatedBiases(null));
16369         assertNull(calibrator.getEstimatedBiasesAsMatrix());
16370         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16371         assertNull(calibrator.getEstimatedBiasFx());
16372         assertNull(calibrator.getEstimatedBiasFy());
16373         assertNull(calibrator.getEstimatedBiasFz());
16374         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16375         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16376         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16377         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16378         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16379         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16380         assertNull(calibrator.getEstimatedMa());
16381         assertNull(calibrator.getEstimatedSx());
16382         assertNull(calibrator.getEstimatedSy());
16383         assertNull(calibrator.getEstimatedSz());
16384         assertNull(calibrator.getEstimatedMxy());
16385         assertNull(calibrator.getEstimatedMxz());
16386         assertNull(calibrator.getEstimatedMyx());
16387         assertNull(calibrator.getEstimatedMyz());
16388         assertNull(calibrator.getEstimatedMzx());
16389         assertNull(calibrator.getEstimatedMzy());
16390         assertNull(calibrator.getEstimatedCovariance());
16391         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16392         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16393                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16394         assertNotNull(calibrator.getGroundTruthGravityNorm());
16395         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16396         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16397         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16398                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16399         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16400         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16401         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16402     }
16403 
16404     @Test
16405     public void testConstructor133() throws WrongSizeException {
16406         final Matrix ba = generateBa();
16407         final double[] bias = ba.getBuffer();
16408         final double biasX = ba.getElementAtIndex(0);
16409         final double biasY = ba.getElementAtIndex(1);
16410         final double biasZ = ba.getElementAtIndex(2);
16411 
16412         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16413         final double latitude = Math.toRadians(
16414                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16415         final double longitude = Math.toRadians(
16416                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16417         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16418         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16419         final NEDVelocity nedVelocity = new NEDVelocity();
16420         final ECEFPosition ecefPosition = new ECEFPosition();
16421         final ECEFVelocity ecefVelocity = new ECEFVelocity();
16422         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16423                 ecefPosition, ecefVelocity);
16424 
16425         KnownPositionAccelerometerCalibrator calibrator =
16426                 new KnownPositionAccelerometerCalibrator(ecefPosition, bias);
16427 
16428         // check default values
16429         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16430         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16431         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16432         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16433         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16434         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16435         final Acceleration bx2 = new Acceleration(0.0,
16436                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16437         calibrator.getInitialBiasXAsAcceleration(bx2);
16438         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16439         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16440         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16441         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16442         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16443         final Acceleration by2 = new Acceleration(0.0,
16444                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16445         calibrator.getInitialBiasYAsAcceleration(by2);
16446         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16447         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16448         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16449         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16450         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16451         final Acceleration bz2 = new Acceleration(0.0,
16452                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16453         calibrator.getInitialBiasZAsAcceleration(bz2);
16454         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16455         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16456         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16457         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16458         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16459         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16460         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16461         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16462         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16463         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16464         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16465         final double[] bias1 = calibrator.getInitialBias();
16466         assertArrayEquals(bias1, bias, 0.0);
16467         final double[] bias2 = new double[3];
16468         calibrator.getInitialBias(bias2);
16469         assertArrayEquals(bias1, bias2, 0.0);
16470         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16471         assertEquals(b1, ba);
16472         final Matrix b2 = new Matrix(3, 1);
16473         calibrator.getInitialBiasAsMatrix(b2);
16474         assertEquals(b1, b2);
16475         final Matrix ma1 = calibrator.getInitialMa();
16476         assertEquals(ma1, new Matrix(3, 3));
16477         final Matrix ma2 = new Matrix(3, 3);
16478         calibrator.getInitialMa(ma2);
16479         assertEquals(ma1, ma2);
16480         assertSame(calibrator.getEcefPosition(), ecefPosition);
16481         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16482         final NEDPosition nedPosition1 = new NEDPosition();
16483         assertTrue(calibrator.getNedPosition(nedPosition1));
16484         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16485         assertNull(calibrator.getMeasurements());
16486         assertFalse(calibrator.isCommonAxisUsed());
16487         assertNull(calibrator.getListener());
16488         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
16489         assertFalse(calibrator.isReady());
16490         assertFalse(calibrator.isRunning());
16491         assertNull(calibrator.getEstimatedBiases());
16492         assertFalse(calibrator.getEstimatedBiases(null));
16493         assertNull(calibrator.getEstimatedBiasesAsMatrix());
16494         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16495         assertNull(calibrator.getEstimatedBiasFx());
16496         assertNull(calibrator.getEstimatedBiasFy());
16497         assertNull(calibrator.getEstimatedBiasFz());
16498         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16499         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16500         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16501         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16502         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16503         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16504         assertNull(calibrator.getEstimatedMa());
16505         assertNull(calibrator.getEstimatedSx());
16506         assertNull(calibrator.getEstimatedSy());
16507         assertNull(calibrator.getEstimatedSz());
16508         assertNull(calibrator.getEstimatedMxy());
16509         assertNull(calibrator.getEstimatedMxz());
16510         assertNull(calibrator.getEstimatedMyx());
16511         assertNull(calibrator.getEstimatedMyz());
16512         assertNull(calibrator.getEstimatedMzx());
16513         assertNull(calibrator.getEstimatedMzy());
16514         assertNull(calibrator.getEstimatedCovariance());
16515         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16516         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16517                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16518         assertNotNull(calibrator.getGroundTruthGravityNorm());
16519         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16520         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16521         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16522                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16523         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16524         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16525         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16526 
16527         // Force IllegalArgumentException
16528         calibrator = null;
16529         try {
16530             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
16531                     new double[1]);
16532             fail("IllegalArgumentException expected but not thrown");
16533         } catch (final IllegalArgumentException ignore) {
16534         }
16535         assertNull(calibrator);
16536     }
16537 
16538     @Test
16539     public void testConstructor134() throws WrongSizeException {
16540         final Matrix ba = generateBa();
16541         final double[] bias = ba.getBuffer();
16542         final double biasX = ba.getElementAtIndex(0);
16543         final double biasY = ba.getElementAtIndex(1);
16544         final double biasZ = ba.getElementAtIndex(2);
16545 
16546         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16547         final double latitude = Math.toRadians(
16548                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16549         final double longitude = Math.toRadians(
16550                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16551         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16552         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16553         final NEDVelocity nedVelocity = new NEDVelocity();
16554         final ECEFPosition ecefPosition = new ECEFPosition();
16555         final ECEFVelocity ecefVelocity = new ECEFVelocity();
16556         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16557                 ecefPosition, ecefVelocity);
16558 
16559         KnownPositionAccelerometerCalibrator calibrator =
16560                 new KnownPositionAccelerometerCalibrator(ecefPosition, bias,
16561                         this);
16562 
16563         // check default values
16564         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16565         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16566         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16567         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16568         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16569         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16570         final Acceleration bx2 = new Acceleration(0.0,
16571                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16572         calibrator.getInitialBiasXAsAcceleration(bx2);
16573         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16574         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16575         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16576         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16577         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16578         final Acceleration by2 = new Acceleration(0.0,
16579                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16580         calibrator.getInitialBiasYAsAcceleration(by2);
16581         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16582         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16583         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16584         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16585         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16586         final Acceleration bz2 = new Acceleration(0.0,
16587                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16588         calibrator.getInitialBiasZAsAcceleration(bz2);
16589         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16590         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16591         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16592         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16593         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16594         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16595         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16596         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16597         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16598         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16599         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16600         final double[] bias1 = calibrator.getInitialBias();
16601         assertArrayEquals(bias1, bias, 0.0);
16602         final double[] bias2 = new double[3];
16603         calibrator.getInitialBias(bias2);
16604         assertArrayEquals(bias1, bias2, 0.0);
16605         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16606         assertEquals(b1, ba);
16607         final Matrix b2 = new Matrix(3, 1);
16608         calibrator.getInitialBiasAsMatrix(b2);
16609         assertEquals(b1, b2);
16610         final Matrix ma1 = calibrator.getInitialMa();
16611         assertEquals(ma1, new Matrix(3, 3));
16612         final Matrix ma2 = new Matrix(3, 3);
16613         calibrator.getInitialMa(ma2);
16614         assertEquals(ma1, ma2);
16615         assertSame(calibrator.getEcefPosition(), ecefPosition);
16616         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16617         final NEDPosition nedPosition1 = new NEDPosition();
16618         assertTrue(calibrator.getNedPosition(nedPosition1));
16619         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16620         assertNull(calibrator.getMeasurements());
16621         assertFalse(calibrator.isCommonAxisUsed());
16622         assertSame(calibrator.getListener(), this);
16623         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
16624         assertFalse(calibrator.isReady());
16625         assertFalse(calibrator.isRunning());
16626         assertNull(calibrator.getEstimatedBiases());
16627         assertFalse(calibrator.getEstimatedBiases(null));
16628         assertNull(calibrator.getEstimatedBiasesAsMatrix());
16629         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16630         assertNull(calibrator.getEstimatedBiasFx());
16631         assertNull(calibrator.getEstimatedBiasFy());
16632         assertNull(calibrator.getEstimatedBiasFz());
16633         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16634         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16635         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16636         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16637         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16638         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16639         assertNull(calibrator.getEstimatedMa());
16640         assertNull(calibrator.getEstimatedSx());
16641         assertNull(calibrator.getEstimatedSy());
16642         assertNull(calibrator.getEstimatedSz());
16643         assertNull(calibrator.getEstimatedMxy());
16644         assertNull(calibrator.getEstimatedMxz());
16645         assertNull(calibrator.getEstimatedMyx());
16646         assertNull(calibrator.getEstimatedMyz());
16647         assertNull(calibrator.getEstimatedMzx());
16648         assertNull(calibrator.getEstimatedMzy());
16649         assertNull(calibrator.getEstimatedCovariance());
16650         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16651         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16652                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16653         assertNotNull(calibrator.getGroundTruthGravityNorm());
16654         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16655         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16656         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16657                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16658         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16659         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16660         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16661 
16662         // Force IllegalArgumentException
16663         calibrator = null;
16664         try {
16665             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
16666                     new double[1], this);
16667             fail("IllegalArgumentException expected but not thrown");
16668         } catch (final IllegalArgumentException ignore) {
16669         }
16670         assertNull(calibrator);
16671     }
16672 
16673     @Test
16674     public void testConstructor135() throws WrongSizeException {
16675         final Collection<StandardDeviationBodyKinematics> measurements =
16676                 Collections.emptyList();
16677 
16678         final Matrix ba = generateBa();
16679         final double[] bias = ba.getBuffer();
16680         final double biasX = ba.getElementAtIndex(0);
16681         final double biasY = ba.getElementAtIndex(1);
16682         final double biasZ = ba.getElementAtIndex(2);
16683 
16684         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16685         final double latitude = Math.toRadians(
16686                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16687         final double longitude = Math.toRadians(
16688                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16689         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16690         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16691         final NEDVelocity nedVelocity = new NEDVelocity();
16692         final ECEFPosition ecefPosition = new ECEFPosition();
16693         final ECEFVelocity ecefVelocity = new ECEFVelocity();
16694         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16695                 ecefPosition, ecefVelocity);
16696 
16697         KnownPositionAccelerometerCalibrator calibrator =
16698                 new KnownPositionAccelerometerCalibrator(ecefPosition,
16699                         measurements, bias);
16700 
16701         // check default values
16702         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16703         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16704         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16705         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16706         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16707         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16708         final Acceleration bx2 = new Acceleration(0.0,
16709                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16710         calibrator.getInitialBiasXAsAcceleration(bx2);
16711         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16712         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16713         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16714         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16715         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16716         final Acceleration by2 = new Acceleration(0.0,
16717                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16718         calibrator.getInitialBiasYAsAcceleration(by2);
16719         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16720         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16721         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16722         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16723         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16724         final Acceleration bz2 = new Acceleration(0.0,
16725                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16726         calibrator.getInitialBiasZAsAcceleration(bz2);
16727         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16728         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16729         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16730         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16731         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16732         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16733         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16734         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16735         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16736         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16737         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16738         final double[] bias1 = calibrator.getInitialBias();
16739         assertArrayEquals(bias1, bias, 0.0);
16740         final double[] bias2 = new double[3];
16741         calibrator.getInitialBias(bias2);
16742         assertArrayEquals(bias1, bias2, 0.0);
16743         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16744         assertEquals(b1, ba);
16745         final Matrix b2 = new Matrix(3, 1);
16746         calibrator.getInitialBiasAsMatrix(b2);
16747         assertEquals(b1, b2);
16748         final Matrix ma1 = calibrator.getInitialMa();
16749         assertEquals(ma1, new Matrix(3, 3));
16750         final Matrix ma2 = new Matrix(3, 3);
16751         calibrator.getInitialMa(ma2);
16752         assertEquals(ma1, ma2);
16753         assertSame(calibrator.getEcefPosition(), ecefPosition);
16754         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16755         final NEDPosition nedPosition1 = new NEDPosition();
16756         assertTrue(calibrator.getNedPosition(nedPosition1));
16757         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16758         assertSame(calibrator.getMeasurements(), measurements);
16759         assertFalse(calibrator.isCommonAxisUsed());
16760         assertNull(calibrator.getListener());
16761         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
16762         assertFalse(calibrator.isReady());
16763         assertFalse(calibrator.isRunning());
16764         assertNull(calibrator.getEstimatedBiases());
16765         assertFalse(calibrator.getEstimatedBiases(null));
16766         assertNull(calibrator.getEstimatedBiasesAsMatrix());
16767         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16768         assertNull(calibrator.getEstimatedBiasFx());
16769         assertNull(calibrator.getEstimatedBiasFy());
16770         assertNull(calibrator.getEstimatedBiasFz());
16771         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16772         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16773         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16774         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16775         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16776         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16777         assertNull(calibrator.getEstimatedMa());
16778         assertNull(calibrator.getEstimatedSx());
16779         assertNull(calibrator.getEstimatedSy());
16780         assertNull(calibrator.getEstimatedSz());
16781         assertNull(calibrator.getEstimatedMxy());
16782         assertNull(calibrator.getEstimatedMxz());
16783         assertNull(calibrator.getEstimatedMyx());
16784         assertNull(calibrator.getEstimatedMyz());
16785         assertNull(calibrator.getEstimatedMzx());
16786         assertNull(calibrator.getEstimatedMzy());
16787         assertNull(calibrator.getEstimatedCovariance());
16788         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16789         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16790                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16791         assertNotNull(calibrator.getGroundTruthGravityNorm());
16792         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16793         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16794         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16795                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16796         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16797         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16798         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16799 
16800         // Force IllegalArgumentException
16801         calibrator = null;
16802         try {
16803             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
16804                     measurements, new double[1]);
16805             fail("IllegalArgumentException expected but not thrown");
16806         } catch (final IllegalArgumentException ignore) {
16807         }
16808         assertNull(calibrator);
16809     }
16810 
16811     @Test
16812     public void testConstructor136() throws WrongSizeException {
16813         final Collection<StandardDeviationBodyKinematics> measurements =
16814                 Collections.emptyList();
16815 
16816         final Matrix ba = generateBa();
16817         final double[] bias = ba.getBuffer();
16818         final double biasX = ba.getElementAtIndex(0);
16819         final double biasY = ba.getElementAtIndex(1);
16820         final double biasZ = ba.getElementAtIndex(2);
16821 
16822         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16823         final double latitude = Math.toRadians(
16824                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16825         final double longitude = Math.toRadians(
16826                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16827         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16828         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16829         final NEDVelocity nedVelocity = new NEDVelocity();
16830         final ECEFPosition ecefPosition = new ECEFPosition();
16831         final ECEFVelocity ecefVelocity = new ECEFVelocity();
16832         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16833                 ecefPosition, ecefVelocity);
16834 
16835         KnownPositionAccelerometerCalibrator calibrator =
16836                 new KnownPositionAccelerometerCalibrator(ecefPosition,
16837                         measurements, bias, this);
16838 
16839         // check default values
16840         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16841         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16842         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16843         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16844         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16845         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16846         final Acceleration bx2 = new Acceleration(0.0,
16847                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16848         calibrator.getInitialBiasXAsAcceleration(bx2);
16849         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16850         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16851         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16852         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16853         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16854         final Acceleration by2 = new Acceleration(0.0,
16855                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16856         calibrator.getInitialBiasYAsAcceleration(by2);
16857         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16858         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16859         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16860         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16861         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16862         final Acceleration bz2 = new Acceleration(0.0,
16863                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16864         calibrator.getInitialBiasZAsAcceleration(bz2);
16865         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16866         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16867         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16868         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16869         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16870         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16871         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16872         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16873         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16874         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16875         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16876         final double[] bias1 = calibrator.getInitialBias();
16877         assertArrayEquals(bias1, bias, 0.0);
16878         final double[] bias2 = new double[3];
16879         calibrator.getInitialBias(bias2);
16880         assertArrayEquals(bias1, bias2, 0.0);
16881         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16882         assertEquals(b1, ba);
16883         final Matrix b2 = new Matrix(3, 1);
16884         calibrator.getInitialBiasAsMatrix(b2);
16885         assertEquals(b1, b2);
16886         final Matrix ma1 = calibrator.getInitialMa();
16887         assertEquals(ma1, new Matrix(3, 3));
16888         final Matrix ma2 = new Matrix(3, 3);
16889         calibrator.getInitialMa(ma2);
16890         assertEquals(ma1, ma2);
16891         assertSame(calibrator.getEcefPosition(), ecefPosition);
16892         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16893         final NEDPosition nedPosition1 = new NEDPosition();
16894         assertTrue(calibrator.getNedPosition(nedPosition1));
16895         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16896         assertSame(calibrator.getMeasurements(), measurements);
16897         assertFalse(calibrator.isCommonAxisUsed());
16898         assertSame(calibrator.getListener(), this);
16899         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
16900         assertFalse(calibrator.isReady());
16901         assertFalse(calibrator.isRunning());
16902         assertNull(calibrator.getEstimatedBiases());
16903         assertFalse(calibrator.getEstimatedBiases(null));
16904         assertNull(calibrator.getEstimatedBiasesAsMatrix());
16905         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16906         assertNull(calibrator.getEstimatedBiasFx());
16907         assertNull(calibrator.getEstimatedBiasFy());
16908         assertNull(calibrator.getEstimatedBiasFz());
16909         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16910         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16911         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16912         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16913         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16914         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16915         assertNull(calibrator.getEstimatedMa());
16916         assertNull(calibrator.getEstimatedSx());
16917         assertNull(calibrator.getEstimatedSy());
16918         assertNull(calibrator.getEstimatedSz());
16919         assertNull(calibrator.getEstimatedMxy());
16920         assertNull(calibrator.getEstimatedMxz());
16921         assertNull(calibrator.getEstimatedMyx());
16922         assertNull(calibrator.getEstimatedMyz());
16923         assertNull(calibrator.getEstimatedMzx());
16924         assertNull(calibrator.getEstimatedMzy());
16925         assertNull(calibrator.getEstimatedCovariance());
16926         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16927         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16928                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16929         assertNotNull(calibrator.getGroundTruthGravityNorm());
16930         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16931         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16932         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16933                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16934         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16935         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16936         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16937 
16938         // Force IllegalArgumentException
16939         calibrator = null;
16940         try {
16941             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
16942                     measurements, new double[1], this);
16943             fail("IllegalArgumentException expected but not thrown");
16944         } catch (final IllegalArgumentException ignore) {
16945         }
16946         assertNull(calibrator);
16947     }
16948 
16949     @Test
16950     public void testConstructor137() throws WrongSizeException {
16951         final Matrix ba = generateBa();
16952         final double[] bias = ba.getBuffer();
16953         final double biasX = ba.getElementAtIndex(0);
16954         final double biasY = ba.getElementAtIndex(1);
16955         final double biasZ = ba.getElementAtIndex(2);
16956 
16957         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16958         final double latitude = Math.toRadians(
16959                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16960         final double longitude = Math.toRadians(
16961                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16962         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16963         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16964         final NEDVelocity nedVelocity = new NEDVelocity();
16965         final ECEFPosition ecefPosition = new ECEFPosition();
16966         final ECEFVelocity ecefVelocity = new ECEFVelocity();
16967         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16968                 ecefPosition, ecefVelocity);
16969 
16970         KnownPositionAccelerometerCalibrator calibrator =
16971                 new KnownPositionAccelerometerCalibrator(ecefPosition,
16972                         true, bias);
16973 
16974         // check default values
16975         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16976         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16977         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16978         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16979         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16980         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16981         final Acceleration bx2 = new Acceleration(0.0,
16982                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16983         calibrator.getInitialBiasXAsAcceleration(bx2);
16984         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16985         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16986         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16987         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16988         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16989         final Acceleration by2 = new Acceleration(0.0,
16990                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16991         calibrator.getInitialBiasYAsAcceleration(by2);
16992         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16993         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16994         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16995         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16996         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16997         final Acceleration bz2 = new Acceleration(0.0,
16998                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16999         calibrator.getInitialBiasZAsAcceleration(bz2);
17000         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17001         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17002         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17003         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17004         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17005         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17006         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17007         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17008         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17009         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17010         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17011         final double[] bias1 = calibrator.getInitialBias();
17012         assertArrayEquals(bias1, bias, 0.0);
17013         final double[] bias2 = new double[3];
17014         calibrator.getInitialBias(bias2);
17015         assertArrayEquals(bias1, bias2, 0.0);
17016         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17017         assertEquals(b1, ba);
17018         final Matrix b2 = new Matrix(3, 1);
17019         calibrator.getInitialBiasAsMatrix(b2);
17020         assertEquals(b1, b2);
17021         final Matrix ma1 = calibrator.getInitialMa();
17022         assertEquals(ma1, new Matrix(3, 3));
17023         final Matrix ma2 = new Matrix(3, 3);
17024         calibrator.getInitialMa(ma2);
17025         assertEquals(ma1, ma2);
17026         assertSame(calibrator.getEcefPosition(), ecefPosition);
17027         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17028         final NEDPosition nedPosition1 = new NEDPosition();
17029         assertTrue(calibrator.getNedPosition(nedPosition1));
17030         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17031         assertNull(calibrator.getMeasurements());
17032         assertTrue(calibrator.isCommonAxisUsed());
17033         assertNull(calibrator.getListener());
17034         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
17035         assertFalse(calibrator.isReady());
17036         assertFalse(calibrator.isRunning());
17037         assertNull(calibrator.getEstimatedBiases());
17038         assertFalse(calibrator.getEstimatedBiases(null));
17039         assertNull(calibrator.getEstimatedBiasesAsMatrix());
17040         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17041         assertNull(calibrator.getEstimatedBiasFx());
17042         assertNull(calibrator.getEstimatedBiasFy());
17043         assertNull(calibrator.getEstimatedBiasFz());
17044         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17045         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17046         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17047         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17048         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17049         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17050         assertNull(calibrator.getEstimatedMa());
17051         assertNull(calibrator.getEstimatedSx());
17052         assertNull(calibrator.getEstimatedSy());
17053         assertNull(calibrator.getEstimatedSz());
17054         assertNull(calibrator.getEstimatedMxy());
17055         assertNull(calibrator.getEstimatedMxz());
17056         assertNull(calibrator.getEstimatedMyx());
17057         assertNull(calibrator.getEstimatedMyz());
17058         assertNull(calibrator.getEstimatedMzx());
17059         assertNull(calibrator.getEstimatedMzy());
17060         assertNull(calibrator.getEstimatedCovariance());
17061         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17062         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17063                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17064         assertNotNull(calibrator.getGroundTruthGravityNorm());
17065         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17066         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17067         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17068                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17069         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17070         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17071         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17072 
17073         // Force IllegalArgumentException
17074         calibrator = null;
17075         try {
17076             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17077                     true, new double[1]);
17078             fail("IllegalArgumentException expected but not thrown");
17079         } catch (final IllegalArgumentException ignore) {
17080         }
17081         assertNull(calibrator);
17082     }
17083 
17084     @Test
17085     public void testConstructor138() throws WrongSizeException {
17086         final Matrix ba = generateBa();
17087         final double[] bias = ba.getBuffer();
17088         final double biasX = ba.getElementAtIndex(0);
17089         final double biasY = ba.getElementAtIndex(1);
17090         final double biasZ = ba.getElementAtIndex(2);
17091 
17092         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17093         final double latitude = Math.toRadians(
17094                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17095         final double longitude = Math.toRadians(
17096                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17097         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17098         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17099         final NEDVelocity nedVelocity = new NEDVelocity();
17100         final ECEFPosition ecefPosition = new ECEFPosition();
17101         final ECEFVelocity ecefVelocity = new ECEFVelocity();
17102         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17103                 ecefPosition, ecefVelocity);
17104 
17105         KnownPositionAccelerometerCalibrator calibrator =
17106                 new KnownPositionAccelerometerCalibrator(ecefPosition,
17107                         true, bias, this);
17108 
17109         // check default values
17110         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17111         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17112         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17113         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17114         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17115         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17116         final Acceleration bx2 = new Acceleration(0.0,
17117                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17118         calibrator.getInitialBiasXAsAcceleration(bx2);
17119         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17120         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17121         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17122         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17123         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17124         final Acceleration by2 = new Acceleration(0.0,
17125                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17126         calibrator.getInitialBiasYAsAcceleration(by2);
17127         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17128         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17129         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17130         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17131         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17132         final Acceleration bz2 = new Acceleration(0.0,
17133                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17134         calibrator.getInitialBiasZAsAcceleration(bz2);
17135         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17136         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17137         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17138         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17139         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17140         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17141         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17142         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17143         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17144         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17145         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17146         final double[] bias1 = calibrator.getInitialBias();
17147         assertArrayEquals(bias1, bias, 0.0);
17148         final double[] bias2 = new double[3];
17149         calibrator.getInitialBias(bias2);
17150         assertArrayEquals(bias1, bias2, 0.0);
17151         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17152         assertEquals(b1, ba);
17153         final Matrix b2 = new Matrix(3, 1);
17154         calibrator.getInitialBiasAsMatrix(b2);
17155         assertEquals(b1, b2);
17156         final Matrix ma1 = calibrator.getInitialMa();
17157         assertEquals(ma1, new Matrix(3, 3));
17158         final Matrix ma2 = new Matrix(3, 3);
17159         calibrator.getInitialMa(ma2);
17160         assertEquals(ma1, ma2);
17161         assertSame(calibrator.getEcefPosition(), ecefPosition);
17162         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17163         final NEDPosition nedPosition1 = new NEDPosition();
17164         assertTrue(calibrator.getNedPosition(nedPosition1));
17165         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17166         assertNull(calibrator.getMeasurements());
17167         assertTrue(calibrator.isCommonAxisUsed());
17168         assertSame(calibrator.getListener(), this);
17169         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
17170         assertFalse(calibrator.isReady());
17171         assertFalse(calibrator.isRunning());
17172         assertNull(calibrator.getEstimatedBiases());
17173         assertFalse(calibrator.getEstimatedBiases(null));
17174         assertNull(calibrator.getEstimatedBiasesAsMatrix());
17175         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17176         assertNull(calibrator.getEstimatedBiasFx());
17177         assertNull(calibrator.getEstimatedBiasFy());
17178         assertNull(calibrator.getEstimatedBiasFz());
17179         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17180         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17181         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17182         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17183         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17184         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17185         assertNull(calibrator.getEstimatedMa());
17186         assertNull(calibrator.getEstimatedSx());
17187         assertNull(calibrator.getEstimatedSy());
17188         assertNull(calibrator.getEstimatedSz());
17189         assertNull(calibrator.getEstimatedMxy());
17190         assertNull(calibrator.getEstimatedMxz());
17191         assertNull(calibrator.getEstimatedMyx());
17192         assertNull(calibrator.getEstimatedMyz());
17193         assertNull(calibrator.getEstimatedMzx());
17194         assertNull(calibrator.getEstimatedMzy());
17195         assertNull(calibrator.getEstimatedCovariance());
17196         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17197         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17198                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17199         assertNotNull(calibrator.getGroundTruthGravityNorm());
17200         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17201         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17202         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17203                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17204         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17205         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17206         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17207 
17208         // Force IllegalArgumentException
17209         calibrator = null;
17210         try {
17211             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17212                     true, new double[1], this);
17213             fail("IllegalArgumentException expected but not thrown");
17214         } catch (final IllegalArgumentException ignore) {
17215         }
17216         assertNull(calibrator);
17217     }
17218 
17219     @Test
17220     public void testConstructor139() throws WrongSizeException {
17221         final Collection<StandardDeviationBodyKinematics> measurements =
17222                 Collections.emptyList();
17223 
17224         final Matrix ba = generateBa();
17225         final double[] bias = ba.getBuffer();
17226         final double biasX = ba.getElementAtIndex(0);
17227         final double biasY = ba.getElementAtIndex(1);
17228         final double biasZ = ba.getElementAtIndex(2);
17229 
17230         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17231         final double latitude = Math.toRadians(
17232                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17233         final double longitude = Math.toRadians(
17234                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17235         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17236         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17237         final NEDVelocity nedVelocity = new NEDVelocity();
17238         final ECEFPosition ecefPosition = new ECEFPosition();
17239         final ECEFVelocity ecefVelocity = new ECEFVelocity();
17240         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17241                 ecefPosition, ecefVelocity);
17242 
17243         KnownPositionAccelerometerCalibrator calibrator =
17244                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
17245                         true, bias);
17246 
17247         // check default values
17248         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17249         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17250         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17251         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17252         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17253         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17254         final Acceleration bx2 = new Acceleration(0.0,
17255                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17256         calibrator.getInitialBiasXAsAcceleration(bx2);
17257         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17258         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17259         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17260         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17261         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17262         final Acceleration by2 = new Acceleration(0.0,
17263                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17264         calibrator.getInitialBiasYAsAcceleration(by2);
17265         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17266         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17267         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17268         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17269         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17270         final Acceleration bz2 = new Acceleration(0.0,
17271                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17272         calibrator.getInitialBiasZAsAcceleration(bz2);
17273         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17274         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17275         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17276         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17277         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17278         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17279         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17280         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17281         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17282         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17283         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17284         final double[] bias1 = calibrator.getInitialBias();
17285         assertArrayEquals(bias1, bias, 0.0);
17286         final double[] bias2 = new double[3];
17287         calibrator.getInitialBias(bias2);
17288         assertArrayEquals(bias1, bias2, 0.0);
17289         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17290         assertEquals(b1, ba);
17291         final Matrix b2 = new Matrix(3, 1);
17292         calibrator.getInitialBiasAsMatrix(b2);
17293         assertEquals(b1, b2);
17294         final Matrix ma1 = calibrator.getInitialMa();
17295         assertEquals(ma1, new Matrix(3, 3));
17296         final Matrix ma2 = new Matrix(3, 3);
17297         calibrator.getInitialMa(ma2);
17298         assertEquals(ma1, ma2);
17299         assertSame(calibrator.getEcefPosition(), ecefPosition);
17300         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17301         final NEDPosition nedPosition1 = new NEDPosition();
17302         assertTrue(calibrator.getNedPosition(nedPosition1));
17303         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17304         assertSame(calibrator.getMeasurements(), measurements);
17305         assertTrue(calibrator.isCommonAxisUsed());
17306         assertNull(calibrator.getListener());
17307         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
17308         assertFalse(calibrator.isReady());
17309         assertFalse(calibrator.isRunning());
17310         assertNull(calibrator.getEstimatedBiases());
17311         assertFalse(calibrator.getEstimatedBiases(null));
17312         assertNull(calibrator.getEstimatedBiasesAsMatrix());
17313         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17314         assertNull(calibrator.getEstimatedBiasFx());
17315         assertNull(calibrator.getEstimatedBiasFy());
17316         assertNull(calibrator.getEstimatedBiasFz());
17317         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17318         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17319         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17320         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17321         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17322         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17323         assertNull(calibrator.getEstimatedMa());
17324         assertNull(calibrator.getEstimatedSx());
17325         assertNull(calibrator.getEstimatedSy());
17326         assertNull(calibrator.getEstimatedSz());
17327         assertNull(calibrator.getEstimatedMxy());
17328         assertNull(calibrator.getEstimatedMxz());
17329         assertNull(calibrator.getEstimatedMyx());
17330         assertNull(calibrator.getEstimatedMyz());
17331         assertNull(calibrator.getEstimatedMzx());
17332         assertNull(calibrator.getEstimatedMzy());
17333         assertNull(calibrator.getEstimatedCovariance());
17334         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17335         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17336                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17337         assertNotNull(calibrator.getGroundTruthGravityNorm());
17338         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17339         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17340         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17341                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17342         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17343         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17344         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17345 
17346         // Force IllegalArgumentException
17347         calibrator = null;
17348         try {
17349             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17350                     measurements, true, new double[1]);
17351             fail("IllegalArgumentException expected but not thrown");
17352         } catch (final IllegalArgumentException ignore) {
17353         }
17354         assertNull(calibrator);
17355     }
17356 
17357     @Test
17358     public void testConstructor140() throws WrongSizeException {
17359         final Collection<StandardDeviationBodyKinematics> measurements =
17360                 Collections.emptyList();
17361 
17362         final Matrix ba = generateBa();
17363         final double[] bias = ba.getBuffer();
17364         final double biasX = ba.getElementAtIndex(0);
17365         final double biasY = ba.getElementAtIndex(1);
17366         final double biasZ = ba.getElementAtIndex(2);
17367 
17368         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17369         final double latitude = Math.toRadians(
17370                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17371         final double longitude = Math.toRadians(
17372                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17373         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17374         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17375         final NEDVelocity nedVelocity = new NEDVelocity();
17376         final ECEFPosition ecefPosition = new ECEFPosition();
17377         final ECEFVelocity ecefVelocity = new ECEFVelocity();
17378         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17379                 ecefPosition, ecefVelocity);
17380 
17381         KnownPositionAccelerometerCalibrator calibrator =
17382                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
17383                         true, bias, this);
17384 
17385         // check default values
17386         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17387         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17388         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17389         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17390         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17391         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17392         final Acceleration bx2 = new Acceleration(0.0,
17393                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17394         calibrator.getInitialBiasXAsAcceleration(bx2);
17395         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17396         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17397         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17398         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17399         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17400         final Acceleration by2 = new Acceleration(0.0,
17401                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17402         calibrator.getInitialBiasYAsAcceleration(by2);
17403         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17404         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17405         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17406         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17407         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17408         final Acceleration bz2 = new Acceleration(0.0,
17409                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17410         calibrator.getInitialBiasZAsAcceleration(bz2);
17411         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17412         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17413         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17414         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17415         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17416         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17417         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17418         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17419         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17420         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17421         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17422         final double[] bias1 = calibrator.getInitialBias();
17423         assertArrayEquals(bias1, bias, 0.0);
17424         final double[] bias2 = new double[3];
17425         calibrator.getInitialBias(bias2);
17426         assertArrayEquals(bias1, bias2, 0.0);
17427         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17428         assertEquals(b1, ba);
17429         final Matrix b2 = new Matrix(3, 1);
17430         calibrator.getInitialBiasAsMatrix(b2);
17431         assertEquals(b1, b2);
17432         final Matrix ma1 = calibrator.getInitialMa();
17433         assertEquals(ma1, new Matrix(3, 3));
17434         final Matrix ma2 = new Matrix(3, 3);
17435         calibrator.getInitialMa(ma2);
17436         assertEquals(ma1, ma2);
17437         assertSame(calibrator.getEcefPosition(), ecefPosition);
17438         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17439         final NEDPosition nedPosition1 = new NEDPosition();
17440         assertTrue(calibrator.getNedPosition(nedPosition1));
17441         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17442         assertSame(calibrator.getMeasurements(), measurements);
17443         assertTrue(calibrator.isCommonAxisUsed());
17444         assertSame(calibrator.getListener(), this);
17445         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
17446         assertFalse(calibrator.isReady());
17447         assertFalse(calibrator.isRunning());
17448         assertNull(calibrator.getEstimatedBiases());
17449         assertFalse(calibrator.getEstimatedBiases(null));
17450         assertNull(calibrator.getEstimatedBiasesAsMatrix());
17451         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17452         assertNull(calibrator.getEstimatedBiasFx());
17453         assertNull(calibrator.getEstimatedBiasFy());
17454         assertNull(calibrator.getEstimatedBiasFz());
17455         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17456         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17457         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17458         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17459         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17460         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17461         assertNull(calibrator.getEstimatedMa());
17462         assertNull(calibrator.getEstimatedSx());
17463         assertNull(calibrator.getEstimatedSy());
17464         assertNull(calibrator.getEstimatedSz());
17465         assertNull(calibrator.getEstimatedMxy());
17466         assertNull(calibrator.getEstimatedMxz());
17467         assertNull(calibrator.getEstimatedMyx());
17468         assertNull(calibrator.getEstimatedMyz());
17469         assertNull(calibrator.getEstimatedMzx());
17470         assertNull(calibrator.getEstimatedMzy());
17471         assertNull(calibrator.getEstimatedCovariance());
17472         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17473         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17474                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17475         assertNotNull(calibrator.getGroundTruthGravityNorm());
17476         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17477         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17478         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17479                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17480         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17481         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17482         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17483 
17484         // Force IllegalArgumentException
17485         calibrator = null;
17486         try {
17487             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17488                     measurements, true, new double[1], this);
17489             fail("IllegalArgumentException expected but not thrown");
17490         } catch (final IllegalArgumentException ignore) {
17491         }
17492         assertNull(calibrator);
17493     }
17494 
17495     @Test
17496     public void testConstructor141() throws WrongSizeException {
17497         final Matrix ba = generateBa();
17498         final double[] bias = ba.getBuffer();
17499         final double biasX = ba.getElementAtIndex(0);
17500         final double biasY = ba.getElementAtIndex(1);
17501         final double biasZ = ba.getElementAtIndex(2);
17502 
17503         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17504         final double latitude = Math.toRadians(
17505                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17506         final double longitude = Math.toRadians(
17507                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17508         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17509         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17510         final NEDVelocity nedVelocity = new NEDVelocity();
17511         final ECEFPosition ecefPosition = new ECEFPosition();
17512         final ECEFVelocity ecefVelocity = new ECEFVelocity();
17513         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17514                 ecefPosition, ecefVelocity);
17515 
17516         KnownPositionAccelerometerCalibrator calibrator =
17517                 new KnownPositionAccelerometerCalibrator(ecefPosition, ba);
17518 
17519         // check default values
17520         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17521         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17522         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17523         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17524         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17525         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17526         final Acceleration bx2 = new Acceleration(0.0,
17527                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17528         calibrator.getInitialBiasXAsAcceleration(bx2);
17529         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17530         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17531         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17532         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17533         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17534         final Acceleration by2 = new Acceleration(0.0,
17535                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17536         calibrator.getInitialBiasYAsAcceleration(by2);
17537         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17538         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17539         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17540         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17541         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17542         final Acceleration bz2 = new Acceleration(0.0,
17543                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17544         calibrator.getInitialBiasZAsAcceleration(bz2);
17545         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17546         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17547         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17548         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17549         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17550         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17551         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17552         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17553         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17554         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17555         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17556         final double[] bias1 = calibrator.getInitialBias();
17557         assertArrayEquals(bias1, bias, 0.0);
17558         final double[] bias2 = new double[3];
17559         calibrator.getInitialBias(bias2);
17560         assertArrayEquals(bias1, bias2, 0.0);
17561         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17562         assertEquals(b1, ba);
17563         final Matrix b2 = new Matrix(3, 1);
17564         calibrator.getInitialBiasAsMatrix(b2);
17565         assertEquals(b1, b2);
17566         final Matrix ma1 = calibrator.getInitialMa();
17567         assertEquals(ma1, new Matrix(3, 3));
17568         final Matrix ma2 = new Matrix(3, 3);
17569         calibrator.getInitialMa(ma2);
17570         assertEquals(ma1, ma2);
17571         assertSame(calibrator.getEcefPosition(), ecefPosition);
17572         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17573         final NEDPosition nedPosition1 = new NEDPosition();
17574         assertTrue(calibrator.getNedPosition(nedPosition1));
17575         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17576         assertNull(calibrator.getMeasurements());
17577         assertFalse(calibrator.isCommonAxisUsed());
17578         assertNull(calibrator.getListener());
17579         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
17580         assertFalse(calibrator.isReady());
17581         assertFalse(calibrator.isRunning());
17582         assertNull(calibrator.getEstimatedBiases());
17583         assertFalse(calibrator.getEstimatedBiases(null));
17584         assertNull(calibrator.getEstimatedBiasesAsMatrix());
17585         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17586         assertNull(calibrator.getEstimatedBiasFx());
17587         assertNull(calibrator.getEstimatedBiasFy());
17588         assertNull(calibrator.getEstimatedBiasFz());
17589         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17590         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17591         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17592         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17593         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17594         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17595         assertNull(calibrator.getEstimatedMa());
17596         assertNull(calibrator.getEstimatedSx());
17597         assertNull(calibrator.getEstimatedSy());
17598         assertNull(calibrator.getEstimatedSz());
17599         assertNull(calibrator.getEstimatedMxy());
17600         assertNull(calibrator.getEstimatedMxz());
17601         assertNull(calibrator.getEstimatedMyx());
17602         assertNull(calibrator.getEstimatedMyz());
17603         assertNull(calibrator.getEstimatedMzx());
17604         assertNull(calibrator.getEstimatedMzy());
17605         assertNull(calibrator.getEstimatedCovariance());
17606         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17607         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17608                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17609         assertNotNull(calibrator.getGroundTruthGravityNorm());
17610         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17611         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17612         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17613                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17614         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17615         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17616         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17617 
17618         // Force IllegalArgumentException
17619         calibrator = null;
17620         try {
17621             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17622                     new Matrix(1, 1));
17623             fail("IllegalArgumentException expected but not thrown");
17624         } catch (final IllegalArgumentException ignore) {
17625         }
17626         try {
17627             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17628                     new Matrix(1, 3));
17629             fail("IllegalArgumentException expected but not thrown");
17630         } catch (final IllegalArgumentException ignore) {
17631         }
17632         assertNull(calibrator);
17633     }
17634 
17635     @Test
17636     public void testConstructor142() throws WrongSizeException {
17637         final Matrix ba = generateBa();
17638         final double[] bias = ba.getBuffer();
17639         final double biasX = ba.getElementAtIndex(0);
17640         final double biasY = ba.getElementAtIndex(1);
17641         final double biasZ = ba.getElementAtIndex(2);
17642 
17643         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17644         final double latitude = Math.toRadians(
17645                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17646         final double longitude = Math.toRadians(
17647                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17648         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17649         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17650         final NEDVelocity nedVelocity = new NEDVelocity();
17651         final ECEFPosition ecefPosition = new ECEFPosition();
17652         final ECEFVelocity ecefVelocity = new ECEFVelocity();
17653         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17654                 ecefPosition, ecefVelocity);
17655 
17656         KnownPositionAccelerometerCalibrator calibrator =
17657                 new KnownPositionAccelerometerCalibrator(ecefPosition, ba,
17658                         this);
17659 
17660         // check default values
17661         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17662         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17663         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17664         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17665         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17666         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17667         final Acceleration bx2 = new Acceleration(0.0,
17668                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17669         calibrator.getInitialBiasXAsAcceleration(bx2);
17670         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17671         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17672         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17673         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17674         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17675         final Acceleration by2 = new Acceleration(0.0,
17676                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17677         calibrator.getInitialBiasYAsAcceleration(by2);
17678         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17679         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17680         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17681         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17682         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17683         final Acceleration bz2 = new Acceleration(0.0,
17684                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17685         calibrator.getInitialBiasZAsAcceleration(bz2);
17686         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17687         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17688         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17689         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17690         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17691         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17692         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17693         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17694         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17695         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17696         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17697         final double[] bias1 = calibrator.getInitialBias();
17698         assertArrayEquals(bias1, bias, 0.0);
17699         final double[] bias2 = new double[3];
17700         calibrator.getInitialBias(bias2);
17701         assertArrayEquals(bias1, bias2, 0.0);
17702         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17703         assertEquals(b1, ba);
17704         final Matrix b2 = new Matrix(3, 1);
17705         calibrator.getInitialBiasAsMatrix(b2);
17706         assertEquals(b1, b2);
17707         final Matrix ma1 = calibrator.getInitialMa();
17708         assertEquals(ma1, new Matrix(3, 3));
17709         final Matrix ma2 = new Matrix(3, 3);
17710         calibrator.getInitialMa(ma2);
17711         assertEquals(ma1, ma2);
17712         assertSame(calibrator.getEcefPosition(), ecefPosition);
17713         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17714         final NEDPosition nedPosition1 = new NEDPosition();
17715         assertTrue(calibrator.getNedPosition(nedPosition1));
17716         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17717         assertNull(calibrator.getMeasurements());
17718         assertFalse(calibrator.isCommonAxisUsed());
17719         assertSame(calibrator.getListener(), this);
17720         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
17721         assertFalse(calibrator.isReady());
17722         assertFalse(calibrator.isRunning());
17723         assertNull(calibrator.getEstimatedBiases());
17724         assertFalse(calibrator.getEstimatedBiases(null));
17725         assertNull(calibrator.getEstimatedBiasesAsMatrix());
17726         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17727         assertNull(calibrator.getEstimatedBiasFx());
17728         assertNull(calibrator.getEstimatedBiasFy());
17729         assertNull(calibrator.getEstimatedBiasFz());
17730         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17731         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17732         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17733         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17734         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17735         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17736         assertNull(calibrator.getEstimatedMa());
17737         assertNull(calibrator.getEstimatedSx());
17738         assertNull(calibrator.getEstimatedSy());
17739         assertNull(calibrator.getEstimatedSz());
17740         assertNull(calibrator.getEstimatedMxy());
17741         assertNull(calibrator.getEstimatedMxz());
17742         assertNull(calibrator.getEstimatedMyx());
17743         assertNull(calibrator.getEstimatedMyz());
17744         assertNull(calibrator.getEstimatedMzx());
17745         assertNull(calibrator.getEstimatedMzy());
17746         assertNull(calibrator.getEstimatedCovariance());
17747         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17748         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17749                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17750         assertNotNull(calibrator.getGroundTruthGravityNorm());
17751         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17752         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17753         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17754                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17755         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17756         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17757         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17758 
17759         // Force IllegalArgumentException
17760         calibrator = null;
17761         try {
17762             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17763                     new Matrix(1, 1), this);
17764             fail("IllegalArgumentException expected but not thrown");
17765         } catch (final IllegalArgumentException ignore) {
17766         }
17767         try {
17768             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17769                     new Matrix(1, 3), this);
17770             fail("IllegalArgumentException expected but not thrown");
17771         } catch (final IllegalArgumentException ignore) {
17772         }
17773         assertNull(calibrator);
17774     }
17775 
17776     @Test
17777     public void testConstructor143() throws WrongSizeException {
17778         final Collection<StandardDeviationBodyKinematics> measurements =
17779                 Collections.emptyList();
17780 
17781         final Matrix ba = generateBa();
17782         final double[] bias = ba.getBuffer();
17783         final double biasX = ba.getElementAtIndex(0);
17784         final double biasY = ba.getElementAtIndex(1);
17785         final double biasZ = ba.getElementAtIndex(2);
17786 
17787         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17788         final double latitude = Math.toRadians(
17789                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17790         final double longitude = Math.toRadians(
17791                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17792         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17793         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17794         final NEDVelocity nedVelocity = new NEDVelocity();
17795         final ECEFPosition ecefPosition = new ECEFPosition();
17796         final ECEFVelocity ecefVelocity = new ECEFVelocity();
17797         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17798                 ecefPosition, ecefVelocity);
17799 
17800         KnownPositionAccelerometerCalibrator calibrator =
17801                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
17802                         ba);
17803 
17804         // check default values
17805         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17806         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17807         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17808         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17809         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17810         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17811         final Acceleration bx2 = new Acceleration(0.0,
17812                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17813         calibrator.getInitialBiasXAsAcceleration(bx2);
17814         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17815         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17816         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17817         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17818         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17819         final Acceleration by2 = new Acceleration(0.0,
17820                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17821         calibrator.getInitialBiasYAsAcceleration(by2);
17822         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17823         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17824         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17825         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17826         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17827         final Acceleration bz2 = new Acceleration(0.0,
17828                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17829         calibrator.getInitialBiasZAsAcceleration(bz2);
17830         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17831         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17832         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17833         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17834         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17835         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17836         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17837         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17838         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17839         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17840         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17841         final double[] bias1 = calibrator.getInitialBias();
17842         assertArrayEquals(bias1, bias, 0.0);
17843         final double[] bias2 = new double[3];
17844         calibrator.getInitialBias(bias2);
17845         assertArrayEquals(bias1, bias2, 0.0);
17846         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17847         assertEquals(b1, ba);
17848         final Matrix b2 = new Matrix(3, 1);
17849         calibrator.getInitialBiasAsMatrix(b2);
17850         assertEquals(b1, b2);
17851         final Matrix ma1 = calibrator.getInitialMa();
17852         assertEquals(ma1, new Matrix(3, 3));
17853         final Matrix ma2 = new Matrix(3, 3);
17854         calibrator.getInitialMa(ma2);
17855         assertEquals(ma1, ma2);
17856         assertSame(calibrator.getEcefPosition(), ecefPosition);
17857         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17858         final NEDPosition nedPosition1 = new NEDPosition();
17859         assertTrue(calibrator.getNedPosition(nedPosition1));
17860         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17861         assertSame(calibrator.getMeasurements(), measurements);
17862         assertFalse(calibrator.isCommonAxisUsed());
17863         assertNull(calibrator.getListener());
17864         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
17865         assertFalse(calibrator.isReady());
17866         assertFalse(calibrator.isRunning());
17867         assertNull(calibrator.getEstimatedBiases());
17868         assertFalse(calibrator.getEstimatedBiases(null));
17869         assertNull(calibrator.getEstimatedBiasesAsMatrix());
17870         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17871         assertNull(calibrator.getEstimatedBiasFx());
17872         assertNull(calibrator.getEstimatedBiasFy());
17873         assertNull(calibrator.getEstimatedBiasFz());
17874         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17875         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17876         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17877         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17878         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17879         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17880         assertNull(calibrator.getEstimatedMa());
17881         assertNull(calibrator.getEstimatedSx());
17882         assertNull(calibrator.getEstimatedSy());
17883         assertNull(calibrator.getEstimatedSz());
17884         assertNull(calibrator.getEstimatedMxy());
17885         assertNull(calibrator.getEstimatedMxz());
17886         assertNull(calibrator.getEstimatedMyx());
17887         assertNull(calibrator.getEstimatedMyz());
17888         assertNull(calibrator.getEstimatedMzx());
17889         assertNull(calibrator.getEstimatedMzy());
17890         assertNull(calibrator.getEstimatedCovariance());
17891         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17892         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17893                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17894         assertNotNull(calibrator.getGroundTruthGravityNorm());
17895         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17896         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17897         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17898                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17899         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17900         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17901         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17902 
17903         // Force IllegalArgumentException
17904         calibrator = null;
17905         try {
17906             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17907                     measurements, new Matrix(1, 1));
17908             fail("IllegalArgumentException expected but not thrown");
17909         } catch (final IllegalArgumentException ignore) {
17910         }
17911         try {
17912             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17913                     measurements, new Matrix(1, 3));
17914             fail("IllegalArgumentException expected but not thrown");
17915         } catch (final IllegalArgumentException ignore) {
17916         }
17917         assertNull(calibrator);
17918     }
17919 
17920     @Test
17921     public void testConstructor144() throws WrongSizeException {
17922         final Collection<StandardDeviationBodyKinematics> measurements =
17923                 Collections.emptyList();
17924 
17925         final Matrix ba = generateBa();
17926         final double[] bias = ba.getBuffer();
17927         final double biasX = ba.getElementAtIndex(0);
17928         final double biasY = ba.getElementAtIndex(1);
17929         final double biasZ = ba.getElementAtIndex(2);
17930 
17931         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17932         final double latitude = Math.toRadians(
17933                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17934         final double longitude = Math.toRadians(
17935                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17936         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17937         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17938         final NEDVelocity nedVelocity = new NEDVelocity();
17939         final ECEFPosition ecefPosition = new ECEFPosition();
17940         final ECEFVelocity ecefVelocity = new ECEFVelocity();
17941         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17942                 ecefPosition, ecefVelocity);
17943 
17944         KnownPositionAccelerometerCalibrator calibrator =
17945                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
17946                         ba, this);
17947 
17948         // check default values
17949         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17950         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17951         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17952         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17953         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17954         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17955         final Acceleration bx2 = new Acceleration(0.0,
17956                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17957         calibrator.getInitialBiasXAsAcceleration(bx2);
17958         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17959         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17960         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17961         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17962         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17963         final Acceleration by2 = new Acceleration(0.0,
17964                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17965         calibrator.getInitialBiasYAsAcceleration(by2);
17966         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17967         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17968         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17969         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17970         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17971         final Acceleration bz2 = new Acceleration(0.0,
17972                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17973         calibrator.getInitialBiasZAsAcceleration(bz2);
17974         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17975         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17976         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17977         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17978         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17979         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17980         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17981         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17982         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17983         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17984         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17985         final double[] bias1 = calibrator.getInitialBias();
17986         assertArrayEquals(bias1, bias, 0.0);
17987         final double[] bias2 = new double[3];
17988         calibrator.getInitialBias(bias2);
17989         assertArrayEquals(bias1, bias2, 0.0);
17990         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17991         assertEquals(b1, ba);
17992         final Matrix b2 = new Matrix(3, 1);
17993         calibrator.getInitialBiasAsMatrix(b2);
17994         assertEquals(b1, b2);
17995         final Matrix ma1 = calibrator.getInitialMa();
17996         assertEquals(ma1, new Matrix(3, 3));
17997         final Matrix ma2 = new Matrix(3, 3);
17998         calibrator.getInitialMa(ma2);
17999         assertEquals(ma1, ma2);
18000         assertSame(calibrator.getEcefPosition(), ecefPosition);
18001         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
18002         final NEDPosition nedPosition1 = new NEDPosition();
18003         assertTrue(calibrator.getNedPosition(nedPosition1));
18004         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
18005         assertSame(calibrator.getMeasurements(), measurements);
18006         assertFalse(calibrator.isCommonAxisUsed());
18007         assertSame(calibrator.getListener(), this);
18008         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
18009         assertFalse(calibrator.isReady());
18010         assertFalse(calibrator.isRunning());
18011         assertNull(calibrator.getEstimatedBiases());
18012         assertFalse(calibrator.getEstimatedBiases(null));
18013         assertNull(calibrator.getEstimatedBiasesAsMatrix());
18014         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18015         assertNull(calibrator.getEstimatedBiasFx());
18016         assertNull(calibrator.getEstimatedBiasFy());
18017         assertNull(calibrator.getEstimatedBiasFz());
18018         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18019         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18020         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18021         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18022         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18023         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18024         assertNull(calibrator.getEstimatedMa());
18025         assertNull(calibrator.getEstimatedSx());
18026         assertNull(calibrator.getEstimatedSy());
18027         assertNull(calibrator.getEstimatedSz());
18028         assertNull(calibrator.getEstimatedMxy());
18029         assertNull(calibrator.getEstimatedMxz());
18030         assertNull(calibrator.getEstimatedMyx());
18031         assertNull(calibrator.getEstimatedMyz());
18032         assertNull(calibrator.getEstimatedMzx());
18033         assertNull(calibrator.getEstimatedMzy());
18034         assertNull(calibrator.getEstimatedCovariance());
18035         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18036         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18037                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18038         assertNotNull(calibrator.getGroundTruthGravityNorm());
18039         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18040         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18041         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18042                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18043         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18044         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18045         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18046 
18047         // Force IllegalArgumentException
18048         calibrator = null;
18049         try {
18050             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18051                     measurements, new Matrix(1, 1), this);
18052             fail("IllegalArgumentException expected but not thrown");
18053         } catch (final IllegalArgumentException ignore) {
18054         }
18055         try {
18056             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18057                     measurements, new Matrix(1, 3), this);
18058             fail("IllegalArgumentException expected but not thrown");
18059         } catch (final IllegalArgumentException ignore) {
18060         }
18061         assertNull(calibrator);
18062     }
18063 
18064     @Test
18065     public void testConstructor145() throws WrongSizeException {
18066         final Matrix ba = generateBa();
18067         final double[] bias = ba.getBuffer();
18068         final double biasX = ba.getElementAtIndex(0);
18069         final double biasY = ba.getElementAtIndex(1);
18070         final double biasZ = ba.getElementAtIndex(2);
18071 
18072         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18073         final double latitude = Math.toRadians(
18074                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
18075         final double longitude = Math.toRadians(
18076                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18077         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18078         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18079         final NEDVelocity nedVelocity = new NEDVelocity();
18080         final ECEFPosition ecefPosition = new ECEFPosition();
18081         final ECEFVelocity ecefVelocity = new ECEFVelocity();
18082         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18083                 ecefPosition, ecefVelocity);
18084 
18085         KnownPositionAccelerometerCalibrator calibrator =
18086                 new KnownPositionAccelerometerCalibrator(ecefPosition,
18087                         true, ba);
18088 
18089         // check default values
18090         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18091         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18092         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18093         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18094         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18095         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18096         final Acceleration bx2 = new Acceleration(0.0,
18097                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18098         calibrator.getInitialBiasXAsAcceleration(bx2);
18099         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18100         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18101         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18102         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18103         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18104         final Acceleration by2 = new Acceleration(0.0,
18105                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18106         calibrator.getInitialBiasYAsAcceleration(by2);
18107         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18108         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18109         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18110         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18111         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18112         final Acceleration bz2 = new Acceleration(0.0,
18113                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18114         calibrator.getInitialBiasZAsAcceleration(bz2);
18115         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18116         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18117         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
18118         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
18119         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
18120         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
18121         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
18122         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
18123         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
18124         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
18125         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
18126         final double[] bias1 = calibrator.getInitialBias();
18127         assertArrayEquals(bias1, bias, 0.0);
18128         final double[] bias2 = new double[3];
18129         calibrator.getInitialBias(bias2);
18130         assertArrayEquals(bias1, bias2, 0.0);
18131         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18132         assertEquals(b1, ba);
18133         final Matrix b2 = new Matrix(3, 1);
18134         calibrator.getInitialBiasAsMatrix(b2);
18135         assertEquals(b1, b2);
18136         final Matrix ma1 = calibrator.getInitialMa();
18137         assertEquals(ma1, new Matrix(3, 3));
18138         final Matrix ma2 = new Matrix(3, 3);
18139         calibrator.getInitialMa(ma2);
18140         assertEquals(ma1, ma2);
18141         assertSame(calibrator.getEcefPosition(), ecefPosition);
18142         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
18143         final NEDPosition nedPosition1 = new NEDPosition();
18144         assertTrue(calibrator.getNedPosition(nedPosition1));
18145         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
18146         assertNull(calibrator.getMeasurements());
18147         assertTrue(calibrator.isCommonAxisUsed());
18148         assertNull(calibrator.getListener());
18149         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
18150         assertFalse(calibrator.isReady());
18151         assertFalse(calibrator.isRunning());
18152         assertNull(calibrator.getEstimatedBiases());
18153         assertFalse(calibrator.getEstimatedBiases(null));
18154         assertNull(calibrator.getEstimatedBiasesAsMatrix());
18155         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18156         assertNull(calibrator.getEstimatedBiasFx());
18157         assertNull(calibrator.getEstimatedBiasFy());
18158         assertNull(calibrator.getEstimatedBiasFz());
18159         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18160         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18161         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18162         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18163         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18164         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18165         assertNull(calibrator.getEstimatedMa());
18166         assertNull(calibrator.getEstimatedSx());
18167         assertNull(calibrator.getEstimatedSy());
18168         assertNull(calibrator.getEstimatedSz());
18169         assertNull(calibrator.getEstimatedMxy());
18170         assertNull(calibrator.getEstimatedMxz());
18171         assertNull(calibrator.getEstimatedMyx());
18172         assertNull(calibrator.getEstimatedMyz());
18173         assertNull(calibrator.getEstimatedMzx());
18174         assertNull(calibrator.getEstimatedMzy());
18175         assertNull(calibrator.getEstimatedCovariance());
18176         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18177         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18178                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18179         assertNotNull(calibrator.getGroundTruthGravityNorm());
18180         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18181         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18182         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18183                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18184         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18185         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18186         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18187 
18188         // Force IllegalArgumentException
18189         calibrator = null;
18190         try {
18191             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18192                     true, new Matrix(1, 1));
18193             fail("IllegalArgumentException expected but not thrown");
18194         } catch (final IllegalArgumentException ignore) {
18195         }
18196         try {
18197             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18198                     true, new Matrix(1, 3));
18199             fail("IllegalArgumentException expected but not thrown");
18200         } catch (final IllegalArgumentException ignore) {
18201         }
18202         assertNull(calibrator);
18203     }
18204 
18205     @Test
18206     public void testConstructor146() throws WrongSizeException {
18207         final Matrix ba = generateBa();
18208         final double[] bias = ba.getBuffer();
18209         final double biasX = ba.getElementAtIndex(0);
18210         final double biasY = ba.getElementAtIndex(1);
18211         final double biasZ = ba.getElementAtIndex(2);
18212 
18213         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18214         final double latitude = Math.toRadians(
18215                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
18216         final double longitude = Math.toRadians(
18217                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18218         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18219         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18220         final NEDVelocity nedVelocity = new NEDVelocity();
18221         final ECEFPosition ecefPosition = new ECEFPosition();
18222         final ECEFVelocity ecefVelocity = new ECEFVelocity();
18223         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18224                 ecefPosition, ecefVelocity);
18225 
18226         KnownPositionAccelerometerCalibrator calibrator =
18227                 new KnownPositionAccelerometerCalibrator(ecefPosition,
18228                         true, ba, this);
18229 
18230         // check default values
18231         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18232         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18233         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18234         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18235         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18236         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18237         final Acceleration bx2 = new Acceleration(0.0,
18238                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18239         calibrator.getInitialBiasXAsAcceleration(bx2);
18240         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18241         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18242         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18243         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18244         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18245         final Acceleration by2 = new Acceleration(0.0,
18246                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18247         calibrator.getInitialBiasYAsAcceleration(by2);
18248         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18249         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18250         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18251         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18252         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18253         final Acceleration bz2 = new Acceleration(0.0,
18254                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18255         calibrator.getInitialBiasZAsAcceleration(bz2);
18256         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18257         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18258         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
18259         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
18260         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
18261         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
18262         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
18263         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
18264         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
18265         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
18266         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
18267         final double[] bias1 = calibrator.getInitialBias();
18268         assertArrayEquals(bias1, bias, 0.0);
18269         final double[] bias2 = new double[3];
18270         calibrator.getInitialBias(bias2);
18271         assertArrayEquals(bias1, bias2, 0.0);
18272         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18273         assertEquals(b1, ba);
18274         final Matrix b2 = new Matrix(3, 1);
18275         calibrator.getInitialBiasAsMatrix(b2);
18276         assertEquals(b1, b2);
18277         final Matrix ma1 = calibrator.getInitialMa();
18278         assertEquals(ma1, new Matrix(3, 3));
18279         final Matrix ma2 = new Matrix(3, 3);
18280         calibrator.getInitialMa(ma2);
18281         assertEquals(ma1, ma2);
18282         assertSame(calibrator.getEcefPosition(), ecefPosition);
18283         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
18284         final NEDPosition nedPosition1 = new NEDPosition();
18285         assertTrue(calibrator.getNedPosition(nedPosition1));
18286         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
18287         assertNull(calibrator.getMeasurements());
18288         assertTrue(calibrator.isCommonAxisUsed());
18289         assertSame(calibrator.getListener(), this);
18290         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
18291         assertFalse(calibrator.isReady());
18292         assertFalse(calibrator.isRunning());
18293         assertNull(calibrator.getEstimatedBiases());
18294         assertFalse(calibrator.getEstimatedBiases(null));
18295         assertNull(calibrator.getEstimatedBiasesAsMatrix());
18296         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18297         assertNull(calibrator.getEstimatedBiasFx());
18298         assertNull(calibrator.getEstimatedBiasFy());
18299         assertNull(calibrator.getEstimatedBiasFz());
18300         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18301         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18302         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18303         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18304         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18305         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18306         assertNull(calibrator.getEstimatedMa());
18307         assertNull(calibrator.getEstimatedSx());
18308         assertNull(calibrator.getEstimatedSy());
18309         assertNull(calibrator.getEstimatedSz());
18310         assertNull(calibrator.getEstimatedMxy());
18311         assertNull(calibrator.getEstimatedMxz());
18312         assertNull(calibrator.getEstimatedMyx());
18313         assertNull(calibrator.getEstimatedMyz());
18314         assertNull(calibrator.getEstimatedMzx());
18315         assertNull(calibrator.getEstimatedMzy());
18316         assertNull(calibrator.getEstimatedCovariance());
18317         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18318         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18319                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18320         assertNotNull(calibrator.getGroundTruthGravityNorm());
18321         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18322         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18323         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18324                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18325         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18326         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18327         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18328 
18329         // Force IllegalArgumentException
18330         calibrator = null;
18331         try {
18332             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18333                     true, new Matrix(1, 1),
18334                     this);
18335             fail("IllegalArgumentException expected but not thrown");
18336         } catch (final IllegalArgumentException ignore) {
18337         }
18338         try {
18339             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18340                     true, new Matrix(1, 3),
18341                     this);
18342             fail("IllegalArgumentException expected but not thrown");
18343         } catch (final IllegalArgumentException ignore) {
18344         }
18345         assertNull(calibrator);
18346     }
18347 
18348     @Test
18349     public void testConstructor147() throws WrongSizeException {
18350         final Collection<StandardDeviationBodyKinematics> measurements =
18351                 Collections.emptyList();
18352 
18353         final Matrix ba = generateBa();
18354         final double[] bias = ba.getBuffer();
18355         final double biasX = ba.getElementAtIndex(0);
18356         final double biasY = ba.getElementAtIndex(1);
18357         final double biasZ = ba.getElementAtIndex(2);
18358 
18359         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18360         final double latitude = Math.toRadians(
18361                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
18362         final double longitude = Math.toRadians(
18363                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18364         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18365         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18366         final NEDVelocity nedVelocity = new NEDVelocity();
18367         final ECEFPosition ecefPosition = new ECEFPosition();
18368         final ECEFVelocity ecefVelocity = new ECEFVelocity();
18369         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18370                 ecefPosition, ecefVelocity);
18371 
18372         KnownPositionAccelerometerCalibrator calibrator =
18373                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
18374                         true, ba);
18375 
18376         // check default values
18377         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18378         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18379         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18380         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18381         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18382         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18383         final Acceleration bx2 = new Acceleration(0.0,
18384                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18385         calibrator.getInitialBiasXAsAcceleration(bx2);
18386         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18387         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18388         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18389         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18390         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18391         final Acceleration by2 = new Acceleration(0.0,
18392                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18393         calibrator.getInitialBiasYAsAcceleration(by2);
18394         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18395         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18396         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18397         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18398         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18399         final Acceleration bz2 = new Acceleration(0.0,
18400                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18401         calibrator.getInitialBiasZAsAcceleration(bz2);
18402         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18403         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18404         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
18405         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
18406         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
18407         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
18408         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
18409         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
18410         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
18411         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
18412         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
18413         final double[] bias1 = calibrator.getInitialBias();
18414         assertArrayEquals(bias1, bias, 0.0);
18415         final double[] bias2 = new double[3];
18416         calibrator.getInitialBias(bias2);
18417         assertArrayEquals(bias1, bias2, 0.0);
18418         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18419         assertEquals(b1, ba);
18420         final Matrix b2 = new Matrix(3, 1);
18421         calibrator.getInitialBiasAsMatrix(b2);
18422         assertEquals(b1, b2);
18423         final Matrix ma1 = calibrator.getInitialMa();
18424         assertEquals(ma1, new Matrix(3, 3));
18425         final Matrix ma2 = new Matrix(3, 3);
18426         calibrator.getInitialMa(ma2);
18427         assertEquals(ma1, ma2);
18428         assertSame(calibrator.getEcefPosition(), ecefPosition);
18429         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
18430         final NEDPosition nedPosition1 = new NEDPosition();
18431         assertTrue(calibrator.getNedPosition(nedPosition1));
18432         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
18433         assertSame(calibrator.getMeasurements(), measurements);
18434         assertTrue(calibrator.isCommonAxisUsed());
18435         assertNull(calibrator.getListener());
18436         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
18437         assertFalse(calibrator.isReady());
18438         assertFalse(calibrator.isRunning());
18439         assertNull(calibrator.getEstimatedBiases());
18440         assertFalse(calibrator.getEstimatedBiases(null));
18441         assertNull(calibrator.getEstimatedBiasesAsMatrix());
18442         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18443         assertNull(calibrator.getEstimatedBiasFx());
18444         assertNull(calibrator.getEstimatedBiasFy());
18445         assertNull(calibrator.getEstimatedBiasFz());
18446         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18447         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18448         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18449         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18450         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18451         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18452         assertNull(calibrator.getEstimatedMa());
18453         assertNull(calibrator.getEstimatedSx());
18454         assertNull(calibrator.getEstimatedSy());
18455         assertNull(calibrator.getEstimatedSz());
18456         assertNull(calibrator.getEstimatedMxy());
18457         assertNull(calibrator.getEstimatedMxz());
18458         assertNull(calibrator.getEstimatedMyx());
18459         assertNull(calibrator.getEstimatedMyz());
18460         assertNull(calibrator.getEstimatedMzx());
18461         assertNull(calibrator.getEstimatedMzy());
18462         assertNull(calibrator.getEstimatedCovariance());
18463         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18464         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18465                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18466         assertNotNull(calibrator.getGroundTruthGravityNorm());
18467         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18468         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18469         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18470                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18471         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18472         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18473         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18474 
18475         // Force IllegalArgumentException
18476         calibrator = null;
18477         try {
18478             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18479                     measurements, true,
18480                     new Matrix(1, 1));
18481             fail("IllegalArgumentException expected but not thrown");
18482         } catch (final IllegalArgumentException ignore) {
18483         }
18484         try {
18485             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18486                     measurements, true,
18487                     new Matrix(1, 3));
18488             fail("IllegalArgumentException expected but not thrown");
18489         } catch (final IllegalArgumentException ignore) {
18490         }
18491         assertNull(calibrator);
18492     }
18493 
18494     @Test
18495     public void testConstructor148() throws WrongSizeException {
18496         final Collection<StandardDeviationBodyKinematics> measurements =
18497                 Collections.emptyList();
18498 
18499         final Matrix ba = generateBa();
18500         final double[] bias = ba.getBuffer();
18501         final double biasX = ba.getElementAtIndex(0);
18502         final double biasY = ba.getElementAtIndex(1);
18503         final double biasZ = ba.getElementAtIndex(2);
18504 
18505         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18506         final double latitude = Math.toRadians(
18507                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
18508         final double longitude = Math.toRadians(
18509                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18510         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18511         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18512         final NEDVelocity nedVelocity = new NEDVelocity();
18513         final ECEFPosition ecefPosition = new ECEFPosition();
18514         final ECEFVelocity ecefVelocity = new ECEFVelocity();
18515         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18516                 ecefPosition, ecefVelocity);
18517 
18518         KnownPositionAccelerometerCalibrator calibrator =
18519                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
18520                         true, ba, this);
18521 
18522         // check default values
18523         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18524         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18525         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18526         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18527         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18528         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18529         final Acceleration bx2 = new Acceleration(0.0,
18530                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18531         calibrator.getInitialBiasXAsAcceleration(bx2);
18532         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18533         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18534         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18535         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18536         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18537         final Acceleration by2 = new Acceleration(0.0,
18538                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18539         calibrator.getInitialBiasYAsAcceleration(by2);
18540         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18541         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18542         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18543         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18544         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18545         final Acceleration bz2 = new Acceleration(0.0,
18546                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18547         calibrator.getInitialBiasZAsAcceleration(bz2);
18548         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18549         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18550         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
18551         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
18552         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
18553         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
18554         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
18555         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
18556         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
18557         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
18558         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
18559         final double[] bias1 = calibrator.getInitialBias();
18560         assertArrayEquals(bias1, bias, 0.0);
18561         final double[] bias2 = new double[3];
18562         calibrator.getInitialBias(bias2);
18563         assertArrayEquals(bias1, bias2, 0.0);
18564         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18565         assertEquals(b1, ba);
18566         final Matrix b2 = new Matrix(3, 1);
18567         calibrator.getInitialBiasAsMatrix(b2);
18568         assertEquals(b1, b2);
18569         final Matrix ma1 = calibrator.getInitialMa();
18570         assertEquals(ma1, new Matrix(3, 3));
18571         final Matrix ma2 = new Matrix(3, 3);
18572         calibrator.getInitialMa(ma2);
18573         assertEquals(ma1, ma2);
18574         assertSame(calibrator.getEcefPosition(), ecefPosition);
18575         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
18576         final NEDPosition nedPosition1 = new NEDPosition();
18577         assertTrue(calibrator.getNedPosition(nedPosition1));
18578         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
18579         assertSame(calibrator.getMeasurements(), measurements);
18580         assertTrue(calibrator.isCommonAxisUsed());
18581         assertSame(calibrator.getListener(), this);
18582         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
18583         assertFalse(calibrator.isReady());
18584         assertFalse(calibrator.isRunning());
18585         assertNull(calibrator.getEstimatedBiases());
18586         assertFalse(calibrator.getEstimatedBiases(null));
18587         assertNull(calibrator.getEstimatedBiasesAsMatrix());
18588         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18589         assertNull(calibrator.getEstimatedBiasFx());
18590         assertNull(calibrator.getEstimatedBiasFy());
18591         assertNull(calibrator.getEstimatedBiasFz());
18592         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18593         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18594         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18595         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18596         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18597         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18598         assertNull(calibrator.getEstimatedMa());
18599         assertNull(calibrator.getEstimatedSx());
18600         assertNull(calibrator.getEstimatedSy());
18601         assertNull(calibrator.getEstimatedSz());
18602         assertNull(calibrator.getEstimatedMxy());
18603         assertNull(calibrator.getEstimatedMxz());
18604         assertNull(calibrator.getEstimatedMyx());
18605         assertNull(calibrator.getEstimatedMyz());
18606         assertNull(calibrator.getEstimatedMzx());
18607         assertNull(calibrator.getEstimatedMzy());
18608         assertNull(calibrator.getEstimatedCovariance());
18609         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18610         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18611                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18612         assertNotNull(calibrator.getGroundTruthGravityNorm());
18613         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18614         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18615         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18616                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18617         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18618         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18619         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18620 
18621         // Force IllegalArgumentException
18622         calibrator = null;
18623         try {
18624             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18625                     measurements, true,
18626                     new Matrix(1, 1), this);
18627             fail("IllegalArgumentException expected but not thrown");
18628         } catch (final IllegalArgumentException ignore) {
18629         }
18630         try {
18631             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18632                     measurements, true,
18633                     new Matrix(1, 3), this);
18634             fail("IllegalArgumentException expected but not thrown");
18635         } catch (final IllegalArgumentException ignore) {
18636         }
18637         assertNull(calibrator);
18638     }
18639 
18640     @Test
18641     public void testConstructor149() throws WrongSizeException {
18642         final Matrix ba = generateBa();
18643         final double[] bias = ba.getBuffer();
18644         final double biasX = ba.getElementAtIndex(0);
18645         final double biasY = ba.getElementAtIndex(1);
18646         final double biasZ = ba.getElementAtIndex(2);
18647 
18648         final Matrix ma = generateMaCommonAxis();
18649         final double sx = ma.getElementAt(0, 0);
18650         final double sy = ma.getElementAt(1, 1);
18651         final double sz = ma.getElementAt(2, 2);
18652         final double mxy = ma.getElementAt(0, 1);
18653         final double mxz = ma.getElementAt(0, 2);
18654         final double myx = ma.getElementAt(1, 0);
18655         final double myz = ma.getElementAt(1, 2);
18656         final double mzx = ma.getElementAt(2, 0);
18657         final double mzy = ma.getElementAt(2, 1);
18658 
18659         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18660         final double latitude = Math.toRadians(
18661                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
18662         final double longitude = Math.toRadians(
18663                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18664         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18665         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18666         final NEDVelocity nedVelocity = new NEDVelocity();
18667         final ECEFPosition ecefPosition = new ECEFPosition();
18668         final ECEFVelocity ecefVelocity = new ECEFVelocity();
18669         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18670                 ecefPosition, ecefVelocity);
18671 
18672         KnownPositionAccelerometerCalibrator calibrator =
18673                 new KnownPositionAccelerometerCalibrator(ecefPosition, ba, ma);
18674 
18675         // check default values
18676         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18677         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18678         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18679         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18680         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18681         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18682         final Acceleration bx2 = new Acceleration(0.0,
18683                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18684         calibrator.getInitialBiasXAsAcceleration(bx2);
18685         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18686         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18687         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18688         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18689         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18690         final Acceleration by2 = new Acceleration(0.0,
18691                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18692         calibrator.getInitialBiasYAsAcceleration(by2);
18693         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18694         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18695         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18696         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18697         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18698         final Acceleration bz2 = new Acceleration(0.0,
18699                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18700         calibrator.getInitialBiasZAsAcceleration(bz2);
18701         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18702         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18703         assertEquals(calibrator.getInitialSx(), sx, 0.0);
18704         assertEquals(calibrator.getInitialSy(), sy, 0.0);
18705         assertEquals(calibrator.getInitialSz(), sz, 0.0);
18706         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
18707         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
18708         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
18709         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
18710         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
18711         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
18712         final double[] bias1 = calibrator.getInitialBias();
18713         assertArrayEquals(bias1, bias, 0.0);
18714         final double[] bias2 = new double[3];
18715         calibrator.getInitialBias(bias2);
18716         assertArrayEquals(bias1, bias2, 0.0);
18717         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18718         assertEquals(b1, ba);
18719         final Matrix b2 = new Matrix(3, 1);
18720         calibrator.getInitialBiasAsMatrix(b2);
18721         assertEquals(b1, b2);
18722         final Matrix ma1 = new Matrix(3, 3);
18723         ma1.setSubmatrix(0, 0,
18724                 2, 2,
18725                 new double[]{sx, myx, mzx,
18726                         mxy, sy, mzy,
18727                         mxz, myz, sz});
18728         assertEquals(calibrator.getInitialMa(), ma1);
18729         final Matrix ma2 = new Matrix(3, 3);
18730         calibrator.getInitialMa(ma2);
18731         assertEquals(ma1, ma2);
18732         assertSame(calibrator.getEcefPosition(), ecefPosition);
18733         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
18734         final NEDPosition nedPosition1 = new NEDPosition();
18735         assertTrue(calibrator.getNedPosition(nedPosition1));
18736         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
18737         assertNull(calibrator.getMeasurements());
18738         assertFalse(calibrator.isCommonAxisUsed());
18739         assertNull(calibrator.getListener());
18740         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
18741         assertFalse(calibrator.isReady());
18742         assertFalse(calibrator.isRunning());
18743         assertNull(calibrator.getEstimatedBiases());
18744         assertFalse(calibrator.getEstimatedBiases(null));
18745         assertNull(calibrator.getEstimatedBiasesAsMatrix());
18746         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18747         assertNull(calibrator.getEstimatedBiasFx());
18748         assertNull(calibrator.getEstimatedBiasFy());
18749         assertNull(calibrator.getEstimatedBiasFz());
18750         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18751         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18752         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18753         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18754         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18755         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18756         assertNull(calibrator.getEstimatedMa());
18757         assertNull(calibrator.getEstimatedSx());
18758         assertNull(calibrator.getEstimatedSy());
18759         assertNull(calibrator.getEstimatedSz());
18760         assertNull(calibrator.getEstimatedMxy());
18761         assertNull(calibrator.getEstimatedMxz());
18762         assertNull(calibrator.getEstimatedMyx());
18763         assertNull(calibrator.getEstimatedMyz());
18764         assertNull(calibrator.getEstimatedMzx());
18765         assertNull(calibrator.getEstimatedMzy());
18766         assertNull(calibrator.getEstimatedCovariance());
18767         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18768         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18769                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18770         assertNotNull(calibrator.getGroundTruthGravityNorm());
18771         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18772         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18773         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18774                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18775         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18776         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18777         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18778 
18779         // Force IllegalArgumentException
18780         calibrator = null;
18781         try {
18782             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18783                     new Matrix(1, 1), ma);
18784             fail("IllegalArgumentException expected but not thrown");
18785         } catch (final IllegalArgumentException ignore) {
18786         }
18787         try {
18788             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18789                     new Matrix(1, 3), ma);
18790             fail("IllegalArgumentException expected but not thrown");
18791         } catch (final IllegalArgumentException ignore) {
18792         }
18793         try {
18794             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18795                     ba, new Matrix(1, 3));
18796             fail("IllegalArgumentException expected but not thrown");
18797         } catch (final IllegalArgumentException ignore) {
18798         }
18799         try {
18800             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18801                     ba, new Matrix(3, 1));
18802             fail("IllegalArgumentException expected but not thrown");
18803         } catch (final IllegalArgumentException ignore) {
18804         }
18805         assertNull(calibrator);
18806     }
18807 
18808     @Test
18809     public void testConstructor150() throws WrongSizeException {
18810         final Matrix ba = generateBa();
18811         final double[] bias = ba.getBuffer();
18812         final double biasX = ba.getElementAtIndex(0);
18813         final double biasY = ba.getElementAtIndex(1);
18814         final double biasZ = ba.getElementAtIndex(2);
18815 
18816         final Matrix ma = generateMaCommonAxis();
18817         final double sx = ma.getElementAt(0, 0);
18818         final double sy = ma.getElementAt(1, 1);
18819         final double sz = ma.getElementAt(2, 2);
18820         final double mxy = ma.getElementAt(0, 1);
18821         final double mxz = ma.getElementAt(0, 2);
18822         final double myx = ma.getElementAt(1, 0);
18823         final double myz = ma.getElementAt(1, 2);
18824         final double mzx = ma.getElementAt(2, 0);
18825         final double mzy = ma.getElementAt(2, 1);
18826 
18827         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18828         final double latitude = Math.toRadians(
18829                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
18830         final double longitude = Math.toRadians(
18831                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18832         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18833         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18834         final NEDVelocity nedVelocity = new NEDVelocity();
18835         final ECEFPosition ecefPosition = new ECEFPosition();
18836         final ECEFVelocity ecefVelocity = new ECEFVelocity();
18837         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18838                 ecefPosition, ecefVelocity);
18839 
18840         KnownPositionAccelerometerCalibrator calibrator =
18841                 new KnownPositionAccelerometerCalibrator(ecefPosition,
18842                         ba, ma, this);
18843 
18844         // check default values
18845         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18846         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18847         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18848         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18849         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18850         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18851         final Acceleration bx2 = new Acceleration(0.0,
18852                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18853         calibrator.getInitialBiasXAsAcceleration(bx2);
18854         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18855         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18856         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18857         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18858         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18859         final Acceleration by2 = new Acceleration(0.0,
18860                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18861         calibrator.getInitialBiasYAsAcceleration(by2);
18862         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18863         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18864         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18865         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18866         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18867         final Acceleration bz2 = new Acceleration(0.0,
18868                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18869         calibrator.getInitialBiasZAsAcceleration(bz2);
18870         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18871         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18872         assertEquals(calibrator.getInitialSx(), sx, 0.0);
18873         assertEquals(calibrator.getInitialSy(), sy, 0.0);
18874         assertEquals(calibrator.getInitialSz(), sz, 0.0);
18875         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
18876         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
18877         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
18878         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
18879         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
18880         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
18881         final double[] bias1 = calibrator.getInitialBias();
18882         assertArrayEquals(bias1, bias, 0.0);
18883         final double[] bias2 = new double[3];
18884         calibrator.getInitialBias(bias2);
18885         assertArrayEquals(bias1, bias2, 0.0);
18886         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18887         assertEquals(b1, ba);
18888         final Matrix b2 = new Matrix(3, 1);
18889         calibrator.getInitialBiasAsMatrix(b2);
18890         assertEquals(b1, b2);
18891         final Matrix ma1 = new Matrix(3, 3);
18892         ma1.setSubmatrix(0, 0,
18893                 2, 2,
18894                 new double[]{sx, myx, mzx,
18895                         mxy, sy, mzy,
18896                         mxz, myz, sz});
18897         assertEquals(calibrator.getInitialMa(), ma1);
18898         final Matrix ma2 = new Matrix(3, 3);
18899         calibrator.getInitialMa(ma2);
18900         assertEquals(ma1, ma2);
18901         assertSame(calibrator.getEcefPosition(), ecefPosition);
18902         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
18903         final NEDPosition nedPosition1 = new NEDPosition();
18904         assertTrue(calibrator.getNedPosition(nedPosition1));
18905         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
18906         assertNull(calibrator.getMeasurements());
18907         assertFalse(calibrator.isCommonAxisUsed());
18908         assertSame(calibrator.getListener(), this);
18909         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
18910         assertFalse(calibrator.isReady());
18911         assertFalse(calibrator.isRunning());
18912         assertNull(calibrator.getEstimatedBiases());
18913         assertFalse(calibrator.getEstimatedBiases(null));
18914         assertNull(calibrator.getEstimatedBiasesAsMatrix());
18915         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18916         assertNull(calibrator.getEstimatedBiasFx());
18917         assertNull(calibrator.getEstimatedBiasFy());
18918         assertNull(calibrator.getEstimatedBiasFz());
18919         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18920         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18921         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18922         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18923         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18924         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18925         assertNull(calibrator.getEstimatedMa());
18926         assertNull(calibrator.getEstimatedSx());
18927         assertNull(calibrator.getEstimatedSy());
18928         assertNull(calibrator.getEstimatedSz());
18929         assertNull(calibrator.getEstimatedMxy());
18930         assertNull(calibrator.getEstimatedMxz());
18931         assertNull(calibrator.getEstimatedMyx());
18932         assertNull(calibrator.getEstimatedMyz());
18933         assertNull(calibrator.getEstimatedMzx());
18934         assertNull(calibrator.getEstimatedMzy());
18935         assertNull(calibrator.getEstimatedCovariance());
18936         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18937         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18938                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18939         assertNotNull(calibrator.getGroundTruthGravityNorm());
18940         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18941         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18942         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18943                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18944         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18945         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18946         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18947 
18948         // Force IllegalArgumentException
18949         calibrator = null;
18950         try {
18951             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18952                     new Matrix(1, 1), ma, this);
18953             fail("IllegalArgumentException expected but not thrown");
18954         } catch (final IllegalArgumentException ignore) {
18955         }
18956         try {
18957             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18958                     new Matrix(1, 3), ma, this);
18959             fail("IllegalArgumentException expected but not thrown");
18960         } catch (final IllegalArgumentException ignore) {
18961         }
18962         try {
18963             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18964                     ba, new Matrix(1, 3), this);
18965             fail("IllegalArgumentException expected but not thrown");
18966         } catch (final IllegalArgumentException ignore) {
18967         }
18968         try {
18969             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18970                     ba, new Matrix(3, 1), this);
18971             fail("IllegalArgumentException expected but not thrown");
18972         } catch (final IllegalArgumentException ignore) {
18973         }
18974         assertNull(calibrator);
18975     }
18976 
18977     @Test
18978     public void testConstructor151() throws WrongSizeException {
18979         final Collection<StandardDeviationBodyKinematics> measurements =
18980                 Collections.emptyList();
18981 
18982         final Matrix ba = generateBa();
18983         final double[] bias = ba.getBuffer();
18984         final double biasX = ba.getElementAtIndex(0);
18985         final double biasY = ba.getElementAtIndex(1);
18986         final double biasZ = ba.getElementAtIndex(2);
18987 
18988         final Matrix ma = generateMaCommonAxis();
18989         final double sx = ma.getElementAt(0, 0);
18990         final double sy = ma.getElementAt(1, 1);
18991         final double sz = ma.getElementAt(2, 2);
18992         final double mxy = ma.getElementAt(0, 1);
18993         final double mxz = ma.getElementAt(0, 2);
18994         final double myx = ma.getElementAt(1, 0);
18995         final double myz = ma.getElementAt(1, 2);
18996         final double mzx = ma.getElementAt(2, 0);
18997         final double mzy = ma.getElementAt(2, 1);
18998 
18999         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19000         final double latitude = Math.toRadians(
19001                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
19002         final double longitude = Math.toRadians(
19003                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19004         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19005         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19006         final NEDVelocity nedVelocity = new NEDVelocity();
19007         final ECEFPosition ecefPosition = new ECEFPosition();
19008         final ECEFVelocity ecefVelocity = new ECEFVelocity();
19009         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19010                 ecefPosition, ecefVelocity);
19011 
19012         KnownPositionAccelerometerCalibrator calibrator =
19013                 new KnownPositionAccelerometerCalibrator(ecefPosition,
19014                         measurements, ba, ma);
19015 
19016         // check default values
19017         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
19018         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
19019         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
19020         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19021         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19022         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19023         final Acceleration bx2 = new Acceleration(0.0,
19024                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19025         calibrator.getInitialBiasXAsAcceleration(bx2);
19026         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19027         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19028         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19029         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19030         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19031         final Acceleration by2 = new Acceleration(0.0,
19032                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19033         calibrator.getInitialBiasYAsAcceleration(by2);
19034         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19035         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19036         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19037         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19038         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19039         final Acceleration bz2 = new Acceleration(0.0,
19040                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19041         calibrator.getInitialBiasZAsAcceleration(bz2);
19042         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19043         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19044         assertEquals(calibrator.getInitialSx(), sx, 0.0);
19045         assertEquals(calibrator.getInitialSy(), sy, 0.0);
19046         assertEquals(calibrator.getInitialSz(), sz, 0.0);
19047         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
19048         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
19049         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
19050         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
19051         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
19052         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
19053         final double[] bias1 = calibrator.getInitialBias();
19054         assertArrayEquals(bias1, bias, 0.0);
19055         final double[] bias2 = new double[3];
19056         calibrator.getInitialBias(bias2);
19057         assertArrayEquals(bias1, bias2, 0.0);
19058         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19059         assertEquals(b1, ba);
19060         final Matrix b2 = new Matrix(3, 1);
19061         calibrator.getInitialBiasAsMatrix(b2);
19062         assertEquals(b1, b2);
19063         final Matrix ma1 = new Matrix(3, 3);
19064         ma1.setSubmatrix(0, 0,
19065                 2, 2,
19066                 new double[]{sx, myx, mzx,
19067                         mxy, sy, mzy,
19068                         mxz, myz, sz});
19069         assertEquals(calibrator.getInitialMa(), ma1);
19070         final Matrix ma2 = new Matrix(3, 3);
19071         calibrator.getInitialMa(ma2);
19072         assertEquals(ma1, ma2);
19073         assertSame(calibrator.getEcefPosition(), ecefPosition);
19074         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19075         final NEDPosition nedPosition1 = new NEDPosition();
19076         assertTrue(calibrator.getNedPosition(nedPosition1));
19077         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19078         assertSame(calibrator.getMeasurements(), measurements);
19079         assertFalse(calibrator.isCommonAxisUsed());
19080         assertNull(calibrator.getListener());
19081         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
19082         assertFalse(calibrator.isReady());
19083         assertFalse(calibrator.isRunning());
19084         assertNull(calibrator.getEstimatedBiases());
19085         assertFalse(calibrator.getEstimatedBiases(null));
19086         assertNull(calibrator.getEstimatedBiasesAsMatrix());
19087         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19088         assertNull(calibrator.getEstimatedBiasFx());
19089         assertNull(calibrator.getEstimatedBiasFy());
19090         assertNull(calibrator.getEstimatedBiasFz());
19091         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19092         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19093         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19094         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19095         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19096         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19097         assertNull(calibrator.getEstimatedMa());
19098         assertNull(calibrator.getEstimatedSx());
19099         assertNull(calibrator.getEstimatedSy());
19100         assertNull(calibrator.getEstimatedSz());
19101         assertNull(calibrator.getEstimatedMxy());
19102         assertNull(calibrator.getEstimatedMxz());
19103         assertNull(calibrator.getEstimatedMyx());
19104         assertNull(calibrator.getEstimatedMyz());
19105         assertNull(calibrator.getEstimatedMzx());
19106         assertNull(calibrator.getEstimatedMzy());
19107         assertNull(calibrator.getEstimatedCovariance());
19108         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19109         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19110                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19111         assertNotNull(calibrator.getGroundTruthGravityNorm());
19112         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19113         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19114         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19115                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19116         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19117         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19118         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19119 
19120         // Force IllegalArgumentException
19121         calibrator = null;
19122         try {
19123             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19124                     measurements, new Matrix(1, 1), ma);
19125             fail("IllegalArgumentException expected but not thrown");
19126         } catch (final IllegalArgumentException ignore) {
19127         }
19128         try {
19129             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19130                     measurements, new Matrix(1, 3), ma);
19131             fail("IllegalArgumentException expected but not thrown");
19132         } catch (final IllegalArgumentException ignore) {
19133         }
19134         try {
19135             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19136                     measurements, ba, new Matrix(1, 3));
19137             fail("IllegalArgumentException expected but not thrown");
19138         } catch (final IllegalArgumentException ignore) {
19139         }
19140         try {
19141             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19142                     measurements, ba, new Matrix(3, 1));
19143             fail("IllegalArgumentException expected but not thrown");
19144         } catch (final IllegalArgumentException ignore) {
19145         }
19146         assertNull(calibrator);
19147     }
19148 
19149     @Test
19150     public void testConstructor152() throws WrongSizeException {
19151         final Collection<StandardDeviationBodyKinematics> measurements =
19152                 Collections.emptyList();
19153 
19154         final Matrix ba = generateBa();
19155         final double[] bias = ba.getBuffer();
19156         final double biasX = ba.getElementAtIndex(0);
19157         final double biasY = ba.getElementAtIndex(1);
19158         final double biasZ = ba.getElementAtIndex(2);
19159 
19160         final Matrix ma = generateMaCommonAxis();
19161         final double sx = ma.getElementAt(0, 0);
19162         final double sy = ma.getElementAt(1, 1);
19163         final double sz = ma.getElementAt(2, 2);
19164         final double mxy = ma.getElementAt(0, 1);
19165         final double mxz = ma.getElementAt(0, 2);
19166         final double myx = ma.getElementAt(1, 0);
19167         final double myz = ma.getElementAt(1, 2);
19168         final double mzx = ma.getElementAt(2, 0);
19169         final double mzy = ma.getElementAt(2, 1);
19170 
19171         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19172         final double latitude = Math.toRadians(
19173                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
19174         final double longitude = Math.toRadians(
19175                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19176         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19177         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19178         final NEDVelocity nedVelocity = new NEDVelocity();
19179         final ECEFPosition ecefPosition = new ECEFPosition();
19180         final ECEFVelocity ecefVelocity = new ECEFVelocity();
19181         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19182                 ecefPosition, ecefVelocity);
19183 
19184         KnownPositionAccelerometerCalibrator calibrator =
19185                 new KnownPositionAccelerometerCalibrator(ecefPosition,
19186                         measurements, ba, ma, this);
19187 
19188         // check default values
19189         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
19190         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
19191         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
19192         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19193         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19194         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19195         final Acceleration bx2 = new Acceleration(0.0,
19196                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19197         calibrator.getInitialBiasXAsAcceleration(bx2);
19198         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19199         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19200         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19201         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19202         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19203         final Acceleration by2 = new Acceleration(0.0,
19204                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19205         calibrator.getInitialBiasYAsAcceleration(by2);
19206         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19207         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19208         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19209         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19210         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19211         final Acceleration bz2 = new Acceleration(0.0,
19212                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19213         calibrator.getInitialBiasZAsAcceleration(bz2);
19214         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19215         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19216         assertEquals(calibrator.getInitialSx(), sx, 0.0);
19217         assertEquals(calibrator.getInitialSy(), sy, 0.0);
19218         assertEquals(calibrator.getInitialSz(), sz, 0.0);
19219         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
19220         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
19221         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
19222         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
19223         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
19224         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
19225         final double[] bias1 = calibrator.getInitialBias();
19226         assertArrayEquals(bias1, bias, 0.0);
19227         final double[] bias2 = new double[3];
19228         calibrator.getInitialBias(bias2);
19229         assertArrayEquals(bias1, bias2, 0.0);
19230         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19231         assertEquals(b1, ba);
19232         final Matrix b2 = new Matrix(3, 1);
19233         calibrator.getInitialBiasAsMatrix(b2);
19234         assertEquals(b1, b2);
19235         final Matrix ma1 = new Matrix(3, 3);
19236         ma1.setSubmatrix(0, 0,
19237                 2, 2,
19238                 new double[]{sx, myx, mzx,
19239                         mxy, sy, mzy,
19240                         mxz, myz, sz});
19241         assertEquals(calibrator.getInitialMa(), ma1);
19242         final Matrix ma2 = new Matrix(3, 3);
19243         calibrator.getInitialMa(ma2);
19244         assertEquals(ma1, ma2);
19245         assertSame(calibrator.getEcefPosition(), ecefPosition);
19246         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19247         final NEDPosition nedPosition1 = new NEDPosition();
19248         assertTrue(calibrator.getNedPosition(nedPosition1));
19249         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19250         assertSame(calibrator.getMeasurements(), measurements);
19251         assertFalse(calibrator.isCommonAxisUsed());
19252         assertSame(calibrator.getListener(), this);
19253         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
19254         assertFalse(calibrator.isReady());
19255         assertFalse(calibrator.isRunning());
19256         assertNull(calibrator.getEstimatedBiases());
19257         assertFalse(calibrator.getEstimatedBiases(null));
19258         assertNull(calibrator.getEstimatedBiasesAsMatrix());
19259         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19260         assertNull(calibrator.getEstimatedBiasFx());
19261         assertNull(calibrator.getEstimatedBiasFy());
19262         assertNull(calibrator.getEstimatedBiasFz());
19263         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19264         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19265         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19266         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19267         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19268         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19269         assertNull(calibrator.getEstimatedMa());
19270         assertNull(calibrator.getEstimatedSx());
19271         assertNull(calibrator.getEstimatedSy());
19272         assertNull(calibrator.getEstimatedSz());
19273         assertNull(calibrator.getEstimatedMxy());
19274         assertNull(calibrator.getEstimatedMxz());
19275         assertNull(calibrator.getEstimatedMyx());
19276         assertNull(calibrator.getEstimatedMyz());
19277         assertNull(calibrator.getEstimatedMzx());
19278         assertNull(calibrator.getEstimatedMzy());
19279         assertNull(calibrator.getEstimatedCovariance());
19280         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19281         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19282                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19283         assertNotNull(calibrator.getGroundTruthGravityNorm());
19284         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19285         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19286         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19287                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19288         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19289         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19290         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19291 
19292         // Force IllegalArgumentException
19293         calibrator = null;
19294         try {
19295             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19296                     measurements, new Matrix(1, 1), ma, this);
19297             fail("IllegalArgumentException expected but not thrown");
19298         } catch (final IllegalArgumentException ignore) {
19299         }
19300         try {
19301             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19302                     measurements, new Matrix(1, 3), ma, this);
19303             fail("IllegalArgumentException expected but not thrown");
19304         } catch (final IllegalArgumentException ignore) {
19305         }
19306         try {
19307             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19308                     measurements, ba, new Matrix(1, 3), this);
19309             fail("IllegalArgumentException expected but not thrown");
19310         } catch (final IllegalArgumentException ignore) {
19311         }
19312         try {
19313             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19314                     measurements, ba, new Matrix(3, 1), this);
19315             fail("IllegalArgumentException expected but not thrown");
19316         } catch (final IllegalArgumentException ignore) {
19317         }
19318         assertNull(calibrator);
19319     }
19320 
19321     @Test
19322     public void testConstructor153() throws WrongSizeException {
19323         final Matrix ba = generateBa();
19324         final double[] bias = ba.getBuffer();
19325         final double biasX = ba.getElementAtIndex(0);
19326         final double biasY = ba.getElementAtIndex(1);
19327         final double biasZ = ba.getElementAtIndex(2);
19328 
19329         final Matrix ma = generateMaCommonAxis();
19330         final double sx = ma.getElementAt(0, 0);
19331         final double sy = ma.getElementAt(1, 1);
19332         final double sz = ma.getElementAt(2, 2);
19333         final double mxy = ma.getElementAt(0, 1);
19334         final double mxz = ma.getElementAt(0, 2);
19335         final double myx = ma.getElementAt(1, 0);
19336         final double myz = ma.getElementAt(1, 2);
19337         final double mzx = ma.getElementAt(2, 0);
19338         final double mzy = ma.getElementAt(2, 1);
19339 
19340         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19341         final double latitude = Math.toRadians(
19342                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
19343         final double longitude = Math.toRadians(
19344                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19345         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19346         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19347         final NEDVelocity nedVelocity = new NEDVelocity();
19348         final ECEFPosition ecefPosition = new ECEFPosition();
19349         final ECEFVelocity ecefVelocity = new ECEFVelocity();
19350         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19351                 ecefPosition, ecefVelocity);
19352 
19353         KnownPositionAccelerometerCalibrator calibrator =
19354                 new KnownPositionAccelerometerCalibrator(ecefPosition,
19355                         true, ba, ma);
19356 
19357         // check default values
19358         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
19359         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
19360         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
19361         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19362         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19363         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19364         final Acceleration bx2 = new Acceleration(0.0,
19365                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19366         calibrator.getInitialBiasXAsAcceleration(bx2);
19367         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19368         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19369         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19370         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19371         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19372         final Acceleration by2 = new Acceleration(0.0,
19373                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19374         calibrator.getInitialBiasYAsAcceleration(by2);
19375         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19376         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19377         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19378         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19379         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19380         final Acceleration bz2 = new Acceleration(0.0,
19381                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19382         calibrator.getInitialBiasZAsAcceleration(bz2);
19383         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19384         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19385         assertEquals(calibrator.getInitialSx(), sx, 0.0);
19386         assertEquals(calibrator.getInitialSy(), sy, 0.0);
19387         assertEquals(calibrator.getInitialSz(), sz, 0.0);
19388         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
19389         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
19390         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
19391         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
19392         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
19393         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
19394         final double[] bias1 = calibrator.getInitialBias();
19395         assertArrayEquals(bias1, bias, 0.0);
19396         final double[] bias2 = new double[3];
19397         calibrator.getInitialBias(bias2);
19398         assertArrayEquals(bias1, bias2, 0.0);
19399         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19400         assertEquals(b1, ba);
19401         final Matrix b2 = new Matrix(3, 1);
19402         calibrator.getInitialBiasAsMatrix(b2);
19403         assertEquals(b1, b2);
19404         final Matrix ma1 = new Matrix(3, 3);
19405         ma1.setSubmatrix(0, 0,
19406                 2, 2,
19407                 new double[]{sx, myx, mzx,
19408                         mxy, sy, mzy,
19409                         mxz, myz, sz});
19410         assertEquals(calibrator.getInitialMa(), ma1);
19411         final Matrix ma2 = new Matrix(3, 3);
19412         calibrator.getInitialMa(ma2);
19413         assertEquals(ma1, ma2);
19414         assertSame(calibrator.getEcefPosition(), ecefPosition);
19415         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19416         final NEDPosition nedPosition1 = new NEDPosition();
19417         assertTrue(calibrator.getNedPosition(nedPosition1));
19418         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19419         assertNull(calibrator.getMeasurements());
19420         assertTrue(calibrator.isCommonAxisUsed());
19421         assertNull(calibrator.getListener());
19422         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
19423         assertFalse(calibrator.isReady());
19424         assertFalse(calibrator.isRunning());
19425         assertNull(calibrator.getEstimatedBiases());
19426         assertFalse(calibrator.getEstimatedBiases(null));
19427         assertNull(calibrator.getEstimatedBiasesAsMatrix());
19428         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19429         assertNull(calibrator.getEstimatedBiasFx());
19430         assertNull(calibrator.getEstimatedBiasFy());
19431         assertNull(calibrator.getEstimatedBiasFz());
19432         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19433         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19434         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19435         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19436         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19437         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19438         assertNull(calibrator.getEstimatedMa());
19439         assertNull(calibrator.getEstimatedSx());
19440         assertNull(calibrator.getEstimatedSy());
19441         assertNull(calibrator.getEstimatedSz());
19442         assertNull(calibrator.getEstimatedMxy());
19443         assertNull(calibrator.getEstimatedMxz());
19444         assertNull(calibrator.getEstimatedMyx());
19445         assertNull(calibrator.getEstimatedMyz());
19446         assertNull(calibrator.getEstimatedMzx());
19447         assertNull(calibrator.getEstimatedMzy());
19448         assertNull(calibrator.getEstimatedCovariance());
19449         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19450         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19451                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19452         assertNotNull(calibrator.getGroundTruthGravityNorm());
19453         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19454         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19455         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19456                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19457         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19458         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19459         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19460 
19461         // Force IllegalArgumentException
19462         calibrator = null;
19463         try {
19464             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19465                     true, new Matrix(1, 1), ma);
19466             fail("IllegalArgumentException expected but not thrown");
19467         } catch (final IllegalArgumentException ignore) {
19468         }
19469         try {
19470             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19471                     true, new Matrix(1, 3), ma);
19472             fail("IllegalArgumentException expected but not thrown");
19473         } catch (final IllegalArgumentException ignore) {
19474         }
19475         try {
19476             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19477                     true, ba, new Matrix(1, 3));
19478             fail("IllegalArgumentException expected but not thrown");
19479         } catch (final IllegalArgumentException ignore) {
19480         }
19481         try {
19482             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19483                     true, ba, new Matrix(3, 1));
19484             fail("IllegalArgumentException expected but not thrown");
19485         } catch (final IllegalArgumentException ignore) {
19486         }
19487         assertNull(calibrator);
19488     }
19489 
19490     @Test
19491     public void testConstructor154() throws WrongSizeException {
19492         final Matrix ba = generateBa();
19493         final double[] bias = ba.getBuffer();
19494         final double biasX = ba.getElementAtIndex(0);
19495         final double biasY = ba.getElementAtIndex(1);
19496         final double biasZ = ba.getElementAtIndex(2);
19497 
19498         final Matrix ma = generateMaCommonAxis();
19499         final double sx = ma.getElementAt(0, 0);
19500         final double sy = ma.getElementAt(1, 1);
19501         final double sz = ma.getElementAt(2, 2);
19502         final double mxy = ma.getElementAt(0, 1);
19503         final double mxz = ma.getElementAt(0, 2);
19504         final double myx = ma.getElementAt(1, 0);
19505         final double myz = ma.getElementAt(1, 2);
19506         final double mzx = ma.getElementAt(2, 0);
19507         final double mzy = ma.getElementAt(2, 1);
19508 
19509         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19510         final double latitude = Math.toRadians(
19511                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
19512         final double longitude = Math.toRadians(
19513                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19514         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19515         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19516         final NEDVelocity nedVelocity = new NEDVelocity();
19517         final ECEFPosition ecefPosition = new ECEFPosition();
19518         final ECEFVelocity ecefVelocity = new ECEFVelocity();
19519         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19520                 ecefPosition, ecefVelocity);
19521 
19522         KnownPositionAccelerometerCalibrator calibrator =
19523                 new KnownPositionAccelerometerCalibrator(ecefPosition,
19524                         true, ba, ma, this);
19525 
19526         // check default values
19527         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
19528         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
19529         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
19530         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19531         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19532         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19533         final Acceleration bx2 = new Acceleration(0.0,
19534                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19535         calibrator.getInitialBiasXAsAcceleration(bx2);
19536         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19537         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19538         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19539         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19540         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19541         final Acceleration by2 = new Acceleration(0.0,
19542                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19543         calibrator.getInitialBiasYAsAcceleration(by2);
19544         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19545         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19546         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19547         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19548         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19549         final Acceleration bz2 = new Acceleration(0.0,
19550                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19551         calibrator.getInitialBiasZAsAcceleration(bz2);
19552         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19553         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19554         assertEquals(calibrator.getInitialSx(), sx, 0.0);
19555         assertEquals(calibrator.getInitialSy(), sy, 0.0);
19556         assertEquals(calibrator.getInitialSz(), sz, 0.0);
19557         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
19558         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
19559         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
19560         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
19561         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
19562         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
19563         final double[] bias1 = calibrator.getInitialBias();
19564         assertArrayEquals(bias1, bias, 0.0);
19565         final double[] bias2 = new double[3];
19566         calibrator.getInitialBias(bias2);
19567         assertArrayEquals(bias1, bias2, 0.0);
19568         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19569         assertEquals(b1, ba);
19570         final Matrix b2 = new Matrix(3, 1);
19571         calibrator.getInitialBiasAsMatrix(b2);
19572         assertEquals(b1, b2);
19573         final Matrix ma1 = new Matrix(3, 3);
19574         ma1.setSubmatrix(0, 0,
19575                 2, 2,
19576                 new double[]{sx, myx, mzx,
19577                         mxy, sy, mzy,
19578                         mxz, myz, sz});
19579         assertEquals(calibrator.getInitialMa(), ma1);
19580         final Matrix ma2 = new Matrix(3, 3);
19581         calibrator.getInitialMa(ma2);
19582         assertEquals(ma1, ma2);
19583         assertSame(calibrator.getEcefPosition(), ecefPosition);
19584         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19585         final NEDPosition nedPosition1 = new NEDPosition();
19586         assertTrue(calibrator.getNedPosition(nedPosition1));
19587         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19588         assertNull(calibrator.getMeasurements());
19589         assertTrue(calibrator.isCommonAxisUsed());
19590         assertSame(calibrator.getListener(), this);
19591         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
19592         assertFalse(calibrator.isReady());
19593         assertFalse(calibrator.isRunning());
19594         assertNull(calibrator.getEstimatedBiases());
19595         assertFalse(calibrator.getEstimatedBiases(null));
19596         assertNull(calibrator.getEstimatedBiasesAsMatrix());
19597         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19598         assertNull(calibrator.getEstimatedBiasFx());
19599         assertNull(calibrator.getEstimatedBiasFy());
19600         assertNull(calibrator.getEstimatedBiasFz());
19601         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19602         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19603         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19604         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19605         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19606         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19607         assertNull(calibrator.getEstimatedMa());
19608         assertNull(calibrator.getEstimatedSx());
19609         assertNull(calibrator.getEstimatedSy());
19610         assertNull(calibrator.getEstimatedSz());
19611         assertNull(calibrator.getEstimatedMxy());
19612         assertNull(calibrator.getEstimatedMxz());
19613         assertNull(calibrator.getEstimatedMyx());
19614         assertNull(calibrator.getEstimatedMyz());
19615         assertNull(calibrator.getEstimatedMzx());
19616         assertNull(calibrator.getEstimatedMzy());
19617         assertNull(calibrator.getEstimatedCovariance());
19618         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19619         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19620                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19621         assertNotNull(calibrator.getGroundTruthGravityNorm());
19622         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19623         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19624         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19625                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19626         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19627         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19628         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19629 
19630         // Force IllegalArgumentException
19631         calibrator = null;
19632         try {
19633             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19634                     true, new Matrix(1, 1), ma, this);
19635             fail("IllegalArgumentException expected but not thrown");
19636         } catch (final IllegalArgumentException ignore) {
19637         }
19638         try {
19639             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19640                     true, new Matrix(1, 3), ma, this);
19641             fail("IllegalArgumentException expected but not thrown");
19642         } catch (final IllegalArgumentException ignore) {
19643         }
19644         try {
19645             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19646                     true, ba, new Matrix(1, 3), this);
19647             fail("IllegalArgumentException expected but not thrown");
19648         } catch (final IllegalArgumentException ignore) {
19649         }
19650         try {
19651             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19652                     true, ba, new Matrix(3, 1), this);
19653             fail("IllegalArgumentException expected but not thrown");
19654         } catch (final IllegalArgumentException ignore) {
19655         }
19656         assertNull(calibrator);
19657     }
19658 
19659     @Test
19660     public void testConstructor155() throws WrongSizeException {
19661         final Collection<StandardDeviationBodyKinematics> measurements =
19662                 Collections.emptyList();
19663 
19664         final Matrix ba = generateBa();
19665         final double[] bias = ba.getBuffer();
19666         final double biasX = ba.getElementAtIndex(0);
19667         final double biasY = ba.getElementAtIndex(1);
19668         final double biasZ = ba.getElementAtIndex(2);
19669 
19670         final Matrix ma = generateMaCommonAxis();
19671         final double sx = ma.getElementAt(0, 0);
19672         final double sy = ma.getElementAt(1, 1);
19673         final double sz = ma.getElementAt(2, 2);
19674         final double mxy = ma.getElementAt(0, 1);
19675         final double mxz = ma.getElementAt(0, 2);
19676         final double myx = ma.getElementAt(1, 0);
19677         final double myz = ma.getElementAt(1, 2);
19678         final double mzx = ma.getElementAt(2, 0);
19679         final double mzy = ma.getElementAt(2, 1);
19680 
19681         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19682         final double latitude = Math.toRadians(
19683                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
19684         final double longitude = Math.toRadians(
19685                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19686         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19687         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19688         final NEDVelocity nedVelocity = new NEDVelocity();
19689         final ECEFPosition ecefPosition = new ECEFPosition();
19690         final ECEFVelocity ecefVelocity = new ECEFVelocity();
19691         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19692                 ecefPosition, ecefVelocity);
19693 
19694         KnownPositionAccelerometerCalibrator calibrator =
19695                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
19696                         true, ba, ma);
19697 
19698         // check default values
19699         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
19700         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
19701         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
19702         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19703         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19704         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19705         final Acceleration bx2 = new Acceleration(0.0,
19706                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19707         calibrator.getInitialBiasXAsAcceleration(bx2);
19708         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19709         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19710         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19711         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19712         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19713         final Acceleration by2 = new Acceleration(0.0,
19714                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19715         calibrator.getInitialBiasYAsAcceleration(by2);
19716         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19717         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19718         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19719         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19720         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19721         final Acceleration bz2 = new Acceleration(0.0,
19722                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19723         calibrator.getInitialBiasZAsAcceleration(bz2);
19724         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19725         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19726         assertEquals(calibrator.getInitialSx(), sx, 0.0);
19727         assertEquals(calibrator.getInitialSy(), sy, 0.0);
19728         assertEquals(calibrator.getInitialSz(), sz, 0.0);
19729         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
19730         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
19731         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
19732         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
19733         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
19734         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
19735         final double[] bias1 = calibrator.getInitialBias();
19736         assertArrayEquals(bias1, bias, 0.0);
19737         final double[] bias2 = new double[3];
19738         calibrator.getInitialBias(bias2);
19739         assertArrayEquals(bias1, bias2, 0.0);
19740         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19741         assertEquals(b1, ba);
19742         final Matrix b2 = new Matrix(3, 1);
19743         calibrator.getInitialBiasAsMatrix(b2);
19744         assertEquals(b1, b2);
19745         final Matrix ma1 = new Matrix(3, 3);
19746         ma1.setSubmatrix(0, 0,
19747                 2, 2,
19748                 new double[]{sx, myx, mzx,
19749                         mxy, sy, mzy,
19750                         mxz, myz, sz});
19751         assertEquals(calibrator.getInitialMa(), ma1);
19752         final Matrix ma2 = new Matrix(3, 3);
19753         calibrator.getInitialMa(ma2);
19754         assertEquals(ma1, ma2);
19755         assertSame(calibrator.getEcefPosition(), ecefPosition);
19756         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19757         final NEDPosition nedPosition1 = new NEDPosition();
19758         assertTrue(calibrator.getNedPosition(nedPosition1));
19759         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19760         assertSame(calibrator.getMeasurements(), measurements);
19761         assertTrue(calibrator.isCommonAxisUsed());
19762         assertNull(calibrator.getListener());
19763         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
19764         assertFalse(calibrator.isReady());
19765         assertFalse(calibrator.isRunning());
19766         assertNull(calibrator.getEstimatedBiases());
19767         assertFalse(calibrator.getEstimatedBiases(null));
19768         assertNull(calibrator.getEstimatedBiasesAsMatrix());
19769         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19770         assertNull(calibrator.getEstimatedBiasFx());
19771         assertNull(calibrator.getEstimatedBiasFy());
19772         assertNull(calibrator.getEstimatedBiasFz());
19773         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19774         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19775         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19776         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19777         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19778         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19779         assertNull(calibrator.getEstimatedMa());
19780         assertNull(calibrator.getEstimatedSx());
19781         assertNull(calibrator.getEstimatedSy());
19782         assertNull(calibrator.getEstimatedSz());
19783         assertNull(calibrator.getEstimatedMxy());
19784         assertNull(calibrator.getEstimatedMxz());
19785         assertNull(calibrator.getEstimatedMyx());
19786         assertNull(calibrator.getEstimatedMyz());
19787         assertNull(calibrator.getEstimatedMzx());
19788         assertNull(calibrator.getEstimatedMzy());
19789         assertNull(calibrator.getEstimatedCovariance());
19790         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19791         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19792                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19793         assertNotNull(calibrator.getGroundTruthGravityNorm());
19794         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19795         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19796         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19797                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19798         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19799         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19800         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19801 
19802         // Force IllegalArgumentException
19803         calibrator = null;
19804         try {
19805             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19806                     measurements, true,
19807                     new Matrix(1, 1), ma);
19808             fail("IllegalArgumentException expected but not thrown");
19809         } catch (final IllegalArgumentException ignore) {
19810         }
19811         try {
19812             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19813                     measurements, true,
19814                     new Matrix(1, 3), ma);
19815             fail("IllegalArgumentException expected but not thrown");
19816         } catch (final IllegalArgumentException ignore) {
19817         }
19818         try {
19819             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19820                     measurements, true,
19821                     ba, new Matrix(1, 3));
19822             fail("IllegalArgumentException expected but not thrown");
19823         } catch (final IllegalArgumentException ignore) {
19824         }
19825         try {
19826             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19827                     measurements, true,
19828                     ba, new Matrix(3, 1));
19829             fail("IllegalArgumentException expected but not thrown");
19830         } catch (final IllegalArgumentException ignore) {
19831         }
19832         assertNull(calibrator);
19833     }
19834 
19835     @Test
19836     public void testConstructor156() throws WrongSizeException {
19837         final Collection<StandardDeviationBodyKinematics> measurements =
19838                 Collections.emptyList();
19839 
19840         final Matrix ba = generateBa();
19841         final double[] bias = ba.getBuffer();
19842         final double biasX = ba.getElementAtIndex(0);
19843         final double biasY = ba.getElementAtIndex(1);
19844         final double biasZ = ba.getElementAtIndex(2);
19845 
19846         final Matrix ma = generateMaCommonAxis();
19847         final double sx = ma.getElementAt(0, 0);
19848         final double sy = ma.getElementAt(1, 1);
19849         final double sz = ma.getElementAt(2, 2);
19850         final double mxy = ma.getElementAt(0, 1);
19851         final double mxz = ma.getElementAt(0, 2);
19852         final double myx = ma.getElementAt(1, 0);
19853         final double myz = ma.getElementAt(1, 2);
19854         final double mzx = ma.getElementAt(2, 0);
19855         final double mzy = ma.getElementAt(2, 1);
19856 
19857         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19858         final double latitude = Math.toRadians(
19859                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
19860         final double longitude = Math.toRadians(
19861                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19862         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19863         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19864         final NEDVelocity nedVelocity = new NEDVelocity();
19865         final ECEFPosition ecefPosition = new ECEFPosition();
19866         final ECEFVelocity ecefVelocity = new ECEFVelocity();
19867         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19868                 ecefPosition, ecefVelocity);
19869 
19870         KnownPositionAccelerometerCalibrator calibrator =
19871                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
19872                         true, ba, ma, this);
19873 
19874         // check default values
19875         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
19876         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
19877         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
19878         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19879         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19880         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19881         final Acceleration bx2 = new Acceleration(0.0,
19882                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19883         calibrator.getInitialBiasXAsAcceleration(bx2);
19884         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19885         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19886         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19887         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19888         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19889         final Acceleration by2 = new Acceleration(0.0,
19890                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19891         calibrator.getInitialBiasYAsAcceleration(by2);
19892         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19893         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19894         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19895         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19896         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19897         final Acceleration bz2 = new Acceleration(0.0,
19898                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19899         calibrator.getInitialBiasZAsAcceleration(bz2);
19900         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19901         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19902         assertEquals(calibrator.getInitialSx(), sx, 0.0);
19903         assertEquals(calibrator.getInitialSy(), sy, 0.0);
19904         assertEquals(calibrator.getInitialSz(), sz, 0.0);
19905         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
19906         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
19907         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
19908         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
19909         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
19910         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
19911         final double[] bias1 = calibrator.getInitialBias();
19912         assertArrayEquals(bias1, bias, 0.0);
19913         final double[] bias2 = new double[3];
19914         calibrator.getInitialBias(bias2);
19915         assertArrayEquals(bias1, bias2, 0.0);
19916         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19917         assertEquals(b1, ba);
19918         final Matrix b2 = new Matrix(3, 1);
19919         calibrator.getInitialBiasAsMatrix(b2);
19920         assertEquals(b1, b2);
19921         final Matrix ma1 = new Matrix(3, 3);
19922         ma1.setSubmatrix(0, 0,
19923                 2, 2,
19924                 new double[]{sx, myx, mzx,
19925                         mxy, sy, mzy,
19926                         mxz, myz, sz});
19927         assertEquals(calibrator.getInitialMa(), ma1);
19928         final Matrix ma2 = new Matrix(3, 3);
19929         calibrator.getInitialMa(ma2);
19930         assertEquals(ma1, ma2);
19931         assertSame(calibrator.getEcefPosition(), ecefPosition);
19932         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19933         final NEDPosition nedPosition1 = new NEDPosition();
19934         assertTrue(calibrator.getNedPosition(nedPosition1));
19935         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19936         assertSame(calibrator.getMeasurements(), measurements);
19937         assertTrue(calibrator.isCommonAxisUsed());
19938         assertSame(calibrator.getListener(), this);
19939         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
19940         assertFalse(calibrator.isReady());
19941         assertFalse(calibrator.isRunning());
19942         assertNull(calibrator.getEstimatedBiases());
19943         assertFalse(calibrator.getEstimatedBiases(null));
19944         assertNull(calibrator.getEstimatedBiasesAsMatrix());
19945         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19946         assertNull(calibrator.getEstimatedBiasFx());
19947         assertNull(calibrator.getEstimatedBiasFy());
19948         assertNull(calibrator.getEstimatedBiasFz());
19949         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19950         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19951         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19952         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19953         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19954         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19955         assertNull(calibrator.getEstimatedMa());
19956         assertNull(calibrator.getEstimatedSx());
19957         assertNull(calibrator.getEstimatedSy());
19958         assertNull(calibrator.getEstimatedSz());
19959         assertNull(calibrator.getEstimatedMxy());
19960         assertNull(calibrator.getEstimatedMxz());
19961         assertNull(calibrator.getEstimatedMyx());
19962         assertNull(calibrator.getEstimatedMyz());
19963         assertNull(calibrator.getEstimatedMzx());
19964         assertNull(calibrator.getEstimatedMzy());
19965         assertNull(calibrator.getEstimatedCovariance());
19966         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19967         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19968                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19969         assertNotNull(calibrator.getGroundTruthGravityNorm());
19970         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19971         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19972         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19973                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19974         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19975         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19976         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19977 
19978         // Force IllegalArgumentException
19979         calibrator = null;
19980         try {
19981             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19982                     measurements, true,
19983                     new Matrix(1, 1), ma, this);
19984             fail("IllegalArgumentException expected but not thrown");
19985         } catch (final IllegalArgumentException ignore) {
19986         }
19987         try {
19988             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19989                     measurements, true,
19990                     new Matrix(1, 3), ma, this);
19991             fail("IllegalArgumentException expected but not thrown");
19992         } catch (final IllegalArgumentException ignore) {
19993         }
19994         try {
19995             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19996                     measurements, true,
19997                     ba, new Matrix(1, 3), this);
19998             fail("IllegalArgumentException expected but not thrown");
19999         } catch (final IllegalArgumentException ignore) {
20000         }
20001         try {
20002             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
20003                     measurements, true,
20004                     ba, new Matrix(3, 1), this);
20005             fail("IllegalArgumentException expected but not thrown");
20006         } catch (final IllegalArgumentException ignore) {
20007         }
20008         assertNull(calibrator);
20009     }
20010 
20011     @Test
20012     public void testConstructor157() throws WrongSizeException {
20013         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20014         final double latitude = Math.toRadians(
20015                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20016         final double longitude = Math.toRadians(
20017                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20018         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20019         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20020         final NEDVelocity nedVelocity = new NEDVelocity();
20021         final ECEFPosition ecefPosition = new ECEFPosition();
20022         final ECEFVelocity ecefVelocity = new ECEFVelocity();
20023         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20024                 ecefPosition, ecefVelocity);
20025 
20026         final KnownPositionAccelerometerCalibrator calibrator =
20027                 new KnownPositionAccelerometerCalibrator(nedPosition);
20028 
20029         // check default values
20030         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20031         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20032         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20033         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20034         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20035         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20036         final Acceleration bx2 = new Acceleration(0.0,
20037                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20038         calibrator.getInitialBiasXAsAcceleration(bx2);
20039         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20040         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20041         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20042         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20043         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20044         final Acceleration by2 = new Acceleration(0.0,
20045                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20046         calibrator.getInitialBiasYAsAcceleration(by2);
20047         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20048         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20049         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20050         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20051         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20052         final Acceleration bz2 = new Acceleration(0.0,
20053                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20054         calibrator.getInitialBiasZAsAcceleration(bz2);
20055         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20056         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20057         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20058         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20059         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20060         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20061         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20062         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20063         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20064         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20065         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20066         final double[] bias1 = calibrator.getInitialBias();
20067         assertArrayEquals(bias1, new double[3], 0.0);
20068         final double[] bias2 = new double[3];
20069         calibrator.getInitialBias(bias2);
20070         assertArrayEquals(bias1, bias2, 0.0);
20071         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20072         assertEquals(b1, new Matrix(3, 1));
20073         final Matrix b2 = new Matrix(3, 1);
20074         calibrator.getInitialBiasAsMatrix(b2);
20075         assertEquals(b1, b2);
20076         final Matrix ma1 = calibrator.getInitialMa();
20077         assertEquals(ma1, new Matrix(3, 3));
20078         final Matrix ma2 = new Matrix(3, 3);
20079         calibrator.getInitialMa(ma2);
20080         assertEquals(ma1, ma2);
20081         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20082         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20083         final NEDPosition nedPosition1 = new NEDPosition();
20084         assertTrue(calibrator.getNedPosition(nedPosition1));
20085         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20086         assertNull(calibrator.getMeasurements());
20087         assertFalse(calibrator.isCommonAxisUsed());
20088         assertNull(calibrator.getListener());
20089         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
20090         assertFalse(calibrator.isReady());
20091         assertFalse(calibrator.isRunning());
20092         assertNull(calibrator.getEstimatedBiases());
20093         assertFalse(calibrator.getEstimatedBiases(null));
20094         assertNull(calibrator.getEstimatedBiasesAsMatrix());
20095         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20096         assertNull(calibrator.getEstimatedBiasFx());
20097         assertNull(calibrator.getEstimatedBiasFy());
20098         assertNull(calibrator.getEstimatedBiasFz());
20099         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20100         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20101         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20102         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20103         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20104         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20105         assertNull(calibrator.getEstimatedMa());
20106         assertNull(calibrator.getEstimatedSx());
20107         assertNull(calibrator.getEstimatedSy());
20108         assertNull(calibrator.getEstimatedSz());
20109         assertNull(calibrator.getEstimatedMxy());
20110         assertNull(calibrator.getEstimatedMxz());
20111         assertNull(calibrator.getEstimatedMyx());
20112         assertNull(calibrator.getEstimatedMyz());
20113         assertNull(calibrator.getEstimatedMzx());
20114         assertNull(calibrator.getEstimatedMzy());
20115         assertNull(calibrator.getEstimatedCovariance());
20116         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20117         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20118                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20119         assertNotNull(calibrator.getGroundTruthGravityNorm());
20120         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20121         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20122         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20123                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20124         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20125         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20126         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20127     }
20128 
20129     @Test
20130     public void testConstructor158() throws WrongSizeException {
20131         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20132         final double latitude = Math.toRadians(
20133                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20134         final double longitude = Math.toRadians(
20135                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20136         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20137         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20138         final NEDVelocity nedVelocity = new NEDVelocity();
20139         final ECEFPosition ecefPosition = new ECEFPosition();
20140         final ECEFVelocity ecefVelocity = new ECEFVelocity();
20141         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20142                 ecefPosition, ecefVelocity);
20143 
20144         final KnownPositionAccelerometerCalibrator calibrator =
20145                 new KnownPositionAccelerometerCalibrator(nedPosition, this);
20146 
20147         // check default values
20148         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20149         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20150         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20151         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20152         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20153         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20154         final Acceleration bx2 = new Acceleration(0.0,
20155                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20156         calibrator.getInitialBiasXAsAcceleration(bx2);
20157         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20158         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20159         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20160         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20161         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20162         final Acceleration by2 = new Acceleration(0.0,
20163                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20164         calibrator.getInitialBiasYAsAcceleration(by2);
20165         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20166         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20167         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20168         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20169         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20170         final Acceleration bz2 = new Acceleration(0.0,
20171                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20172         calibrator.getInitialBiasZAsAcceleration(bz2);
20173         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20174         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20175         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20176         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20177         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20178         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20179         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20180         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20181         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20182         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20183         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20184         final double[] bias1 = calibrator.getInitialBias();
20185         assertArrayEquals(bias1, new double[3], 0.0);
20186         final double[] bias2 = new double[3];
20187         calibrator.getInitialBias(bias2);
20188         assertArrayEquals(bias1, bias2, 0.0);
20189         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20190         assertEquals(b1, new Matrix(3, 1));
20191         final Matrix b2 = new Matrix(3, 1);
20192         calibrator.getInitialBiasAsMatrix(b2);
20193         assertEquals(b1, b2);
20194         final Matrix ma1 = calibrator.getInitialMa();
20195         assertEquals(ma1, new Matrix(3, 3));
20196         final Matrix ma2 = new Matrix(3, 3);
20197         calibrator.getInitialMa(ma2);
20198         assertEquals(ma1, ma2);
20199         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20200         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20201         final NEDPosition nedPosition1 = new NEDPosition();
20202         assertTrue(calibrator.getNedPosition(nedPosition1));
20203         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20204         assertNull(calibrator.getMeasurements());
20205         assertFalse(calibrator.isCommonAxisUsed());
20206         assertSame(calibrator.getListener(), this);
20207         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
20208         assertFalse(calibrator.isReady());
20209         assertFalse(calibrator.isRunning());
20210         assertNull(calibrator.getEstimatedBiases());
20211         assertFalse(calibrator.getEstimatedBiases(null));
20212         assertNull(calibrator.getEstimatedBiasesAsMatrix());
20213         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20214         assertNull(calibrator.getEstimatedBiasFx());
20215         assertNull(calibrator.getEstimatedBiasFy());
20216         assertNull(calibrator.getEstimatedBiasFz());
20217         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20218         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20219         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20220         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20221         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20222         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20223         assertNull(calibrator.getEstimatedMa());
20224         assertNull(calibrator.getEstimatedSx());
20225         assertNull(calibrator.getEstimatedSy());
20226         assertNull(calibrator.getEstimatedSz());
20227         assertNull(calibrator.getEstimatedMxy());
20228         assertNull(calibrator.getEstimatedMxz());
20229         assertNull(calibrator.getEstimatedMyx());
20230         assertNull(calibrator.getEstimatedMyz());
20231         assertNull(calibrator.getEstimatedMzx());
20232         assertNull(calibrator.getEstimatedMzy());
20233         assertNull(calibrator.getEstimatedCovariance());
20234         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20235         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20236                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20237         assertNotNull(calibrator.getGroundTruthGravityNorm());
20238         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20239         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20240         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20241                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20242         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20243         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20244         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20245     }
20246 
20247     @Test
20248     public void testConstructor159() throws WrongSizeException {
20249         final Collection<StandardDeviationBodyKinematics> measurements =
20250                 Collections.emptyList();
20251 
20252         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20253         final double latitude = Math.toRadians(
20254                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20255         final double longitude = Math.toRadians(
20256                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20257         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20258         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20259         final NEDVelocity nedVelocity = new NEDVelocity();
20260         final ECEFPosition ecefPosition = new ECEFPosition();
20261         final ECEFVelocity ecefVelocity = new ECEFVelocity();
20262         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20263                 ecefPosition, ecefVelocity);
20264 
20265         final KnownPositionAccelerometerCalibrator calibrator =
20266                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements);
20267 
20268         // check default values
20269         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20270         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20271         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20272         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20273         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20274         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20275         final Acceleration bx2 = new Acceleration(0.0,
20276                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20277         calibrator.getInitialBiasXAsAcceleration(bx2);
20278         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20279         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20280         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20281         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20282         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20283         final Acceleration by2 = new Acceleration(0.0,
20284                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20285         calibrator.getInitialBiasYAsAcceleration(by2);
20286         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20287         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20288         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20289         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20290         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20291         final Acceleration bz2 = new Acceleration(0.0,
20292                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20293         calibrator.getInitialBiasZAsAcceleration(bz2);
20294         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20295         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20296         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20297         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20298         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20299         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20300         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20301         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20302         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20303         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20304         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20305         final double[] bias1 = calibrator.getInitialBias();
20306         assertArrayEquals(bias1, new double[3], 0.0);
20307         final double[] bias2 = new double[3];
20308         calibrator.getInitialBias(bias2);
20309         assertArrayEquals(bias1, bias2, 0.0);
20310         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20311         assertEquals(b1, new Matrix(3, 1));
20312         final Matrix b2 = new Matrix(3, 1);
20313         calibrator.getInitialBiasAsMatrix(b2);
20314         assertEquals(b1, b2);
20315         final Matrix ma1 = calibrator.getInitialMa();
20316         assertEquals(ma1, new Matrix(3, 3));
20317         final Matrix ma2 = new Matrix(3, 3);
20318         calibrator.getInitialMa(ma2);
20319         assertEquals(ma1, ma2);
20320         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20321         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20322         final NEDPosition nedPosition1 = new NEDPosition();
20323         assertTrue(calibrator.getNedPosition(nedPosition1));
20324         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20325         assertSame(calibrator.getMeasurements(), measurements);
20326         assertFalse(calibrator.isCommonAxisUsed());
20327         assertNull(calibrator.getListener());
20328         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
20329         assertFalse(calibrator.isReady());
20330         assertFalse(calibrator.isRunning());
20331         assertNull(calibrator.getEstimatedBiases());
20332         assertFalse(calibrator.getEstimatedBiases(null));
20333         assertNull(calibrator.getEstimatedBiasesAsMatrix());
20334         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20335         assertNull(calibrator.getEstimatedBiasFx());
20336         assertNull(calibrator.getEstimatedBiasFy());
20337         assertNull(calibrator.getEstimatedBiasFz());
20338         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20339         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20340         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20341         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20342         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20343         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20344         assertNull(calibrator.getEstimatedMa());
20345         assertNull(calibrator.getEstimatedSx());
20346         assertNull(calibrator.getEstimatedSy());
20347         assertNull(calibrator.getEstimatedSz());
20348         assertNull(calibrator.getEstimatedMxy());
20349         assertNull(calibrator.getEstimatedMxz());
20350         assertNull(calibrator.getEstimatedMyx());
20351         assertNull(calibrator.getEstimatedMyz());
20352         assertNull(calibrator.getEstimatedMzx());
20353         assertNull(calibrator.getEstimatedMzy());
20354         assertNull(calibrator.getEstimatedCovariance());
20355         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20356         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20357                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20358         assertNotNull(calibrator.getGroundTruthGravityNorm());
20359         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20360         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20361         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20362                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20363         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20364         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20365         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20366     }
20367 
20368     @Test
20369     public void testConstructor160() throws WrongSizeException {
20370         final Collection<StandardDeviationBodyKinematics> measurements =
20371                 Collections.emptyList();
20372 
20373         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20374         final double latitude = Math.toRadians(
20375                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20376         final double longitude = Math.toRadians(
20377                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20378         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20379         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20380         final NEDVelocity nedVelocity = new NEDVelocity();
20381         final ECEFPosition ecefPosition = new ECEFPosition();
20382         final ECEFVelocity ecefVelocity = new ECEFVelocity();
20383         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20384                 ecefPosition, ecefVelocity);
20385 
20386         final KnownPositionAccelerometerCalibrator calibrator =
20387                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
20388                         this);
20389 
20390         // check default values
20391         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20392         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20393         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20394         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20395         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20396         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20397         final Acceleration bx2 = new Acceleration(0.0,
20398                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20399         calibrator.getInitialBiasXAsAcceleration(bx2);
20400         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20401         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20402         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20403         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20404         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20405         final Acceleration by2 = new Acceleration(0.0,
20406                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20407         calibrator.getInitialBiasYAsAcceleration(by2);
20408         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20409         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20410         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20411         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20412         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20413         final Acceleration bz2 = new Acceleration(0.0,
20414                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20415         calibrator.getInitialBiasZAsAcceleration(bz2);
20416         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20417         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20418         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20419         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20420         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20421         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20422         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20423         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20424         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20425         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20426         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20427         final double[] bias1 = calibrator.getInitialBias();
20428         assertArrayEquals(bias1, new double[3], 0.0);
20429         final double[] bias2 = new double[3];
20430         calibrator.getInitialBias(bias2);
20431         assertArrayEquals(bias1, bias2, 0.0);
20432         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20433         assertEquals(b1, new Matrix(3, 1));
20434         final Matrix b2 = new Matrix(3, 1);
20435         calibrator.getInitialBiasAsMatrix(b2);
20436         assertEquals(b1, b2);
20437         final Matrix ma1 = calibrator.getInitialMa();
20438         assertEquals(ma1, new Matrix(3, 3));
20439         final Matrix ma2 = new Matrix(3, 3);
20440         calibrator.getInitialMa(ma2);
20441         assertEquals(ma1, ma2);
20442         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20443         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20444         final NEDPosition nedPosition1 = new NEDPosition();
20445         assertTrue(calibrator.getNedPosition(nedPosition1));
20446         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20447         assertSame(calibrator.getMeasurements(), measurements);
20448         assertFalse(calibrator.isCommonAxisUsed());
20449         assertSame(calibrator.getListener(), this);
20450         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
20451         assertFalse(calibrator.isReady());
20452         assertFalse(calibrator.isRunning());
20453         assertNull(calibrator.getEstimatedBiases());
20454         assertFalse(calibrator.getEstimatedBiases(null));
20455         assertNull(calibrator.getEstimatedBiasesAsMatrix());
20456         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20457         assertNull(calibrator.getEstimatedBiasFx());
20458         assertNull(calibrator.getEstimatedBiasFy());
20459         assertNull(calibrator.getEstimatedBiasFz());
20460         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20461         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20462         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20463         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20464         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20465         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20466         assertNull(calibrator.getEstimatedMa());
20467         assertNull(calibrator.getEstimatedSx());
20468         assertNull(calibrator.getEstimatedSy());
20469         assertNull(calibrator.getEstimatedSz());
20470         assertNull(calibrator.getEstimatedMxy());
20471         assertNull(calibrator.getEstimatedMxz());
20472         assertNull(calibrator.getEstimatedMyx());
20473         assertNull(calibrator.getEstimatedMyz());
20474         assertNull(calibrator.getEstimatedMzx());
20475         assertNull(calibrator.getEstimatedMzy());
20476         assertNull(calibrator.getEstimatedCovariance());
20477         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20478         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20479                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20480         assertNotNull(calibrator.getGroundTruthGravityNorm());
20481         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20482         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20483         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20484                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20485         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20486         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20487         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20488     }
20489 
20490     @Test
20491     public void testConstructor161() throws WrongSizeException {
20492         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20493         final double latitude = Math.toRadians(
20494                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20495         final double longitude = Math.toRadians(
20496                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20497         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20498         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20499         final NEDVelocity nedVelocity = new NEDVelocity();
20500         final ECEFPosition ecefPosition = new ECEFPosition();
20501         final ECEFVelocity ecefVelocity = new ECEFVelocity();
20502         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20503                 ecefPosition, ecefVelocity);
20504 
20505         final KnownPositionAccelerometerCalibrator calibrator =
20506                 new KnownPositionAccelerometerCalibrator(nedPosition,
20507                         true);
20508 
20509         // check default values
20510         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20511         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20512         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20513         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20514         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20515         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20516         final Acceleration bx2 = new Acceleration(0.0,
20517                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20518         calibrator.getInitialBiasXAsAcceleration(bx2);
20519         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20520         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20521         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20522         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20523         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20524         final Acceleration by2 = new Acceleration(0.0,
20525                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20526         calibrator.getInitialBiasYAsAcceleration(by2);
20527         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20528         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20529         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20530         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20531         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20532         final Acceleration bz2 = new Acceleration(0.0,
20533                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20534         calibrator.getInitialBiasZAsAcceleration(bz2);
20535         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20536         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20537         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20538         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20539         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20540         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20541         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20542         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20543         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20544         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20545         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20546         final double[] bias1 = calibrator.getInitialBias();
20547         assertArrayEquals(bias1, new double[3], 0.0);
20548         final double[] bias2 = new double[3];
20549         calibrator.getInitialBias(bias2);
20550         assertArrayEquals(bias1, bias2, 0.0);
20551         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20552         assertEquals(b1, new Matrix(3, 1));
20553         final Matrix b2 = new Matrix(3, 1);
20554         calibrator.getInitialBiasAsMatrix(b2);
20555         assertEquals(b1, b2);
20556         final Matrix ma1 = calibrator.getInitialMa();
20557         assertEquals(ma1, new Matrix(3, 3));
20558         final Matrix ma2 = new Matrix(3, 3);
20559         calibrator.getInitialMa(ma2);
20560         assertEquals(ma1, ma2);
20561         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20562         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20563         final NEDPosition nedPosition1 = new NEDPosition();
20564         assertTrue(calibrator.getNedPosition(nedPosition1));
20565         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20566         assertNull(calibrator.getMeasurements());
20567         assertTrue(calibrator.isCommonAxisUsed());
20568         assertNull(calibrator.getListener());
20569         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
20570         assertFalse(calibrator.isReady());
20571         assertFalse(calibrator.isRunning());
20572         assertNull(calibrator.getEstimatedBiases());
20573         assertFalse(calibrator.getEstimatedBiases(null));
20574         assertNull(calibrator.getEstimatedBiasesAsMatrix());
20575         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20576         assertNull(calibrator.getEstimatedBiasFx());
20577         assertNull(calibrator.getEstimatedBiasFy());
20578         assertNull(calibrator.getEstimatedBiasFz());
20579         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20580         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20581         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20582         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20583         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20584         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20585         assertNull(calibrator.getEstimatedMa());
20586         assertNull(calibrator.getEstimatedSx());
20587         assertNull(calibrator.getEstimatedSy());
20588         assertNull(calibrator.getEstimatedSz());
20589         assertNull(calibrator.getEstimatedMxy());
20590         assertNull(calibrator.getEstimatedMxz());
20591         assertNull(calibrator.getEstimatedMyx());
20592         assertNull(calibrator.getEstimatedMyz());
20593         assertNull(calibrator.getEstimatedMzx());
20594         assertNull(calibrator.getEstimatedMzy());
20595         assertNull(calibrator.getEstimatedCovariance());
20596         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20597         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20598                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20599         assertNotNull(calibrator.getGroundTruthGravityNorm());
20600         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20601         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20602         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20603                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20604         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20605         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20606         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20607     }
20608 
20609     @Test
20610     public void testConstructor162() throws WrongSizeException {
20611         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20612         final double latitude = Math.toRadians(
20613                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20614         final double longitude = Math.toRadians(
20615                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20616         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20617         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20618         final NEDVelocity nedVelocity = new NEDVelocity();
20619         final ECEFPosition ecefPosition = new ECEFPosition();
20620         final ECEFVelocity ecefVelocity = new ECEFVelocity();
20621         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20622                 ecefPosition, ecefVelocity);
20623 
20624         final KnownPositionAccelerometerCalibrator calibrator =
20625                 new KnownPositionAccelerometerCalibrator(nedPosition,
20626                         true, this);
20627 
20628         // check default values
20629         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20630         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20631         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20632         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20633         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20634         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20635         final Acceleration bx2 = new Acceleration(0.0,
20636                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20637         calibrator.getInitialBiasXAsAcceleration(bx2);
20638         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20639         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20640         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20641         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20642         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20643         final Acceleration by2 = new Acceleration(0.0,
20644                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20645         calibrator.getInitialBiasYAsAcceleration(by2);
20646         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20647         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20648         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20649         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20650         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20651         final Acceleration bz2 = new Acceleration(0.0,
20652                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20653         calibrator.getInitialBiasZAsAcceleration(bz2);
20654         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20655         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20656         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20657         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20658         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20659         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20660         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20661         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20662         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20663         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20664         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20665         final double[] bias1 = calibrator.getInitialBias();
20666         assertArrayEquals(bias1, new double[3], 0.0);
20667         final double[] bias2 = new double[3];
20668         calibrator.getInitialBias(bias2);
20669         assertArrayEquals(bias1, bias2, 0.0);
20670         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20671         assertEquals(b1, new Matrix(3, 1));
20672         final Matrix b2 = new Matrix(3, 1);
20673         calibrator.getInitialBiasAsMatrix(b2);
20674         assertEquals(b1, b2);
20675         final Matrix ma1 = calibrator.getInitialMa();
20676         assertEquals(ma1, new Matrix(3, 3));
20677         final Matrix ma2 = new Matrix(3, 3);
20678         calibrator.getInitialMa(ma2);
20679         assertEquals(ma1, ma2);
20680         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20681         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20682         final NEDPosition nedPosition1 = new NEDPosition();
20683         assertTrue(calibrator.getNedPosition(nedPosition1));
20684         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20685         assertNull(calibrator.getMeasurements());
20686         assertTrue(calibrator.isCommonAxisUsed());
20687         assertSame(calibrator.getListener(), this);
20688         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
20689         assertFalse(calibrator.isReady());
20690         assertFalse(calibrator.isRunning());
20691         assertNull(calibrator.getEstimatedBiases());
20692         assertFalse(calibrator.getEstimatedBiases(null));
20693         assertNull(calibrator.getEstimatedBiasesAsMatrix());
20694         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20695         assertNull(calibrator.getEstimatedBiasFx());
20696         assertNull(calibrator.getEstimatedBiasFy());
20697         assertNull(calibrator.getEstimatedBiasFz());
20698         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20699         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20700         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20701         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20702         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20703         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20704         assertNull(calibrator.getEstimatedMa());
20705         assertNull(calibrator.getEstimatedSx());
20706         assertNull(calibrator.getEstimatedSy());
20707         assertNull(calibrator.getEstimatedSz());
20708         assertNull(calibrator.getEstimatedMxy());
20709         assertNull(calibrator.getEstimatedMxz());
20710         assertNull(calibrator.getEstimatedMyx());
20711         assertNull(calibrator.getEstimatedMyz());
20712         assertNull(calibrator.getEstimatedMzx());
20713         assertNull(calibrator.getEstimatedMzy());
20714         assertNull(calibrator.getEstimatedCovariance());
20715         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20716         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20717                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20718         assertNotNull(calibrator.getGroundTruthGravityNorm());
20719         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20720         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20721         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20722                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20723         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20724         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20725         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20726     }
20727 
20728     @Test
20729     public void testConstructor163() throws WrongSizeException {
20730         final Collection<StandardDeviationBodyKinematics> measurements =
20731                 Collections.emptyList();
20732         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20733         final double latitude = Math.toRadians(
20734                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20735         final double longitude = Math.toRadians(
20736                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20737         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20738         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20739         final NEDVelocity nedVelocity = new NEDVelocity();
20740         final ECEFPosition ecefPosition = new ECEFPosition();
20741         final ECEFVelocity ecefVelocity = new ECEFVelocity();
20742         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20743                 ecefPosition, ecefVelocity);
20744 
20745         final KnownPositionAccelerometerCalibrator calibrator =
20746                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
20747                         true);
20748 
20749         // check default values
20750         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20751         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20752         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20753         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20754         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20755         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20756         final Acceleration bx2 = new Acceleration(0.0,
20757                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20758         calibrator.getInitialBiasXAsAcceleration(bx2);
20759         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20760         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20761         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20762         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20763         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20764         final Acceleration by2 = new Acceleration(0.0,
20765                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20766         calibrator.getInitialBiasYAsAcceleration(by2);
20767         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20768         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20769         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20770         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20771         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20772         final Acceleration bz2 = new Acceleration(0.0,
20773                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20774         calibrator.getInitialBiasZAsAcceleration(bz2);
20775         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20776         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20777         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20778         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20779         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20780         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20781         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20782         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20783         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20784         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20785         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20786         final double[] bias1 = calibrator.getInitialBias();
20787         assertArrayEquals(bias1, new double[3], 0.0);
20788         final double[] bias2 = new double[3];
20789         calibrator.getInitialBias(bias2);
20790         assertArrayEquals(bias1, bias2, 0.0);
20791         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20792         assertEquals(b1, new Matrix(3, 1));
20793         final Matrix b2 = new Matrix(3, 1);
20794         calibrator.getInitialBiasAsMatrix(b2);
20795         assertEquals(b1, b2);
20796         final Matrix ma1 = calibrator.getInitialMa();
20797         assertEquals(ma1, new Matrix(3, 3));
20798         final Matrix ma2 = new Matrix(3, 3);
20799         calibrator.getInitialMa(ma2);
20800         assertEquals(ma1, ma2);
20801         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20802         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20803         final NEDPosition nedPosition1 = new NEDPosition();
20804         assertTrue(calibrator.getNedPosition(nedPosition1));
20805         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20806         assertSame(calibrator.getMeasurements(), measurements);
20807         assertTrue(calibrator.isCommonAxisUsed());
20808         assertNull(calibrator.getListener());
20809         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
20810         assertFalse(calibrator.isReady());
20811         assertFalse(calibrator.isRunning());
20812         assertNull(calibrator.getEstimatedBiases());
20813         assertFalse(calibrator.getEstimatedBiases(null));
20814         assertNull(calibrator.getEstimatedBiasesAsMatrix());
20815         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20816         assertNull(calibrator.getEstimatedBiasFx());
20817         assertNull(calibrator.getEstimatedBiasFy());
20818         assertNull(calibrator.getEstimatedBiasFz());
20819         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20820         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20821         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20822         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20823         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20824         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20825         assertNull(calibrator.getEstimatedMa());
20826         assertNull(calibrator.getEstimatedSx());
20827         assertNull(calibrator.getEstimatedSy());
20828         assertNull(calibrator.getEstimatedSz());
20829         assertNull(calibrator.getEstimatedMxy());
20830         assertNull(calibrator.getEstimatedMxz());
20831         assertNull(calibrator.getEstimatedMyx());
20832         assertNull(calibrator.getEstimatedMyz());
20833         assertNull(calibrator.getEstimatedMzx());
20834         assertNull(calibrator.getEstimatedMzy());
20835         assertNull(calibrator.getEstimatedCovariance());
20836         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20837         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20838                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20839         assertNotNull(calibrator.getGroundTruthGravityNorm());
20840         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20841         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20842         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20843                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20844         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20845         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20846         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20847     }
20848 
20849     @Test
20850     public void testConstructor164() throws WrongSizeException {
20851         final Collection<StandardDeviationBodyKinematics> measurements =
20852                 Collections.emptyList();
20853         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20854         final double latitude = Math.toRadians(
20855                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20856         final double longitude = Math.toRadians(
20857                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20858         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20859         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20860         final NEDVelocity nedVelocity = new NEDVelocity();
20861         final ECEFPosition ecefPosition = new ECEFPosition();
20862         final ECEFVelocity ecefVelocity = new ECEFVelocity();
20863         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20864                 ecefPosition, ecefVelocity);
20865 
20866         final KnownPositionAccelerometerCalibrator calibrator =
20867                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
20868                         true, this);
20869 
20870         // check default values
20871         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20872         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20873         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20874         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20875         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20876         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20877         final Acceleration bx2 = new Acceleration(0.0,
20878                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20879         calibrator.getInitialBiasXAsAcceleration(bx2);
20880         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20881         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20882         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20883         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20884         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20885         final Acceleration by2 = new Acceleration(0.0,
20886                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20887         calibrator.getInitialBiasYAsAcceleration(by2);
20888         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20889         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20890         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20891         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20892         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20893         final Acceleration bz2 = new Acceleration(0.0,
20894                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20895         calibrator.getInitialBiasZAsAcceleration(bz2);
20896         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20897         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20898         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20899         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20900         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20901         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20902         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20903         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20904         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20905         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20906         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20907         final double[] bias1 = calibrator.getInitialBias();
20908         assertArrayEquals(bias1, new double[3], 0.0);
20909         final double[] bias2 = new double[3];
20910         calibrator.getInitialBias(bias2);
20911         assertArrayEquals(bias1, bias2, 0.0);
20912         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20913         assertEquals(b1, new Matrix(3, 1));
20914         final Matrix b2 = new Matrix(3, 1);
20915         calibrator.getInitialBiasAsMatrix(b2);
20916         assertEquals(b1, b2);
20917         final Matrix ma1 = calibrator.getInitialMa();
20918         assertEquals(ma1, new Matrix(3, 3));
20919         final Matrix ma2 = new Matrix(3, 3);
20920         calibrator.getInitialMa(ma2);
20921         assertEquals(ma1, ma2);
20922         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20923         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20924         final NEDPosition nedPosition1 = new NEDPosition();
20925         assertTrue(calibrator.getNedPosition(nedPosition1));
20926         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20927         assertSame(calibrator.getMeasurements(), measurements);
20928         assertTrue(calibrator.isCommonAxisUsed());
20929         assertSame(calibrator.getListener(), this);
20930         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
20931         assertFalse(calibrator.isReady());
20932         assertFalse(calibrator.isRunning());
20933         assertNull(calibrator.getEstimatedBiases());
20934         assertFalse(calibrator.getEstimatedBiases(null));
20935         assertNull(calibrator.getEstimatedBiasesAsMatrix());
20936         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20937         assertNull(calibrator.getEstimatedBiasFx());
20938         assertNull(calibrator.getEstimatedBiasFy());
20939         assertNull(calibrator.getEstimatedBiasFz());
20940         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20941         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20942         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20943         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20944         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20945         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20946         assertNull(calibrator.getEstimatedMa());
20947         assertNull(calibrator.getEstimatedSx());
20948         assertNull(calibrator.getEstimatedSy());
20949         assertNull(calibrator.getEstimatedSz());
20950         assertNull(calibrator.getEstimatedMxy());
20951         assertNull(calibrator.getEstimatedMxz());
20952         assertNull(calibrator.getEstimatedMyx());
20953         assertNull(calibrator.getEstimatedMyz());
20954         assertNull(calibrator.getEstimatedMzx());
20955         assertNull(calibrator.getEstimatedMzy());
20956         assertNull(calibrator.getEstimatedCovariance());
20957         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20958         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20959                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20960         assertNotNull(calibrator.getGroundTruthGravityNorm());
20961         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20962         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20963         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20964                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20965         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20966         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20967         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20968     }
20969 
20970     @Test
20971     public void testConstructor165() throws WrongSizeException {
20972         final Matrix ba = generateBa();
20973         final double biasX = ba.getElementAtIndex(0);
20974         final double biasY = ba.getElementAtIndex(1);
20975         final double biasZ = ba.getElementAtIndex(2);
20976 
20977         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20978         final double latitude = Math.toRadians(
20979                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20980         final double longitude = Math.toRadians(
20981                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20982         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20983         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20984         final NEDVelocity nedVelocity = new NEDVelocity();
20985         final ECEFPosition ecefPosition = new ECEFPosition();
20986         final ECEFVelocity ecefVelocity = new ECEFVelocity();
20987         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20988                 ecefPosition, ecefVelocity);
20989 
20990         final KnownPositionAccelerometerCalibrator calibrator =
20991                 new KnownPositionAccelerometerCalibrator(nedPosition,
20992                         biasX, biasY, biasZ);
20993 
20994         // check default values
20995         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
20996         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
20997         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
20998         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20999         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21000         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21001         final Acceleration bx2 = new Acceleration(0.0,
21002                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21003         calibrator.getInitialBiasXAsAcceleration(bx2);
21004         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21005         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21006         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21007         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21008         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21009         final Acceleration by2 = new Acceleration(0.0,
21010                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21011         calibrator.getInitialBiasYAsAcceleration(by2);
21012         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21013         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21014         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21015         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21016         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21017         final Acceleration bz2 = new Acceleration(0.0,
21018                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21019         calibrator.getInitialBiasZAsAcceleration(bz2);
21020         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21021         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21022         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21023         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21024         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21025         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21026         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21027         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21028         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21029         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21030         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21031         final double[] bias1 = calibrator.getInitialBias();
21032         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21033         final double[] bias2 = new double[3];
21034         calibrator.getInitialBias(bias2);
21035         assertArrayEquals(bias1, bias2, 0.0);
21036         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21037         assertEquals(b1, ba);
21038         final Matrix b2 = new Matrix(3, 1);
21039         calibrator.getInitialBiasAsMatrix(b2);
21040         assertEquals(b1, b2);
21041         final Matrix ma1 = calibrator.getInitialMa();
21042         assertEquals(ma1, new Matrix(3, 3));
21043         final Matrix ma2 = new Matrix(3, 3);
21044         calibrator.getInitialMa(ma2);
21045         assertEquals(ma1, ma2);
21046         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21047         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21048         final NEDPosition nedPosition1 = new NEDPosition();
21049         assertTrue(calibrator.getNedPosition(nedPosition1));
21050         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21051         assertNull(calibrator.getMeasurements());
21052         assertFalse(calibrator.isCommonAxisUsed());
21053         assertNull(calibrator.getListener());
21054         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
21055         assertFalse(calibrator.isReady());
21056         assertFalse(calibrator.isRunning());
21057         assertNull(calibrator.getEstimatedBiases());
21058         assertFalse(calibrator.getEstimatedBiases(null));
21059         assertNull(calibrator.getEstimatedBiasesAsMatrix());
21060         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21061         assertNull(calibrator.getEstimatedBiasFx());
21062         assertNull(calibrator.getEstimatedBiasFy());
21063         assertNull(calibrator.getEstimatedBiasFz());
21064         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21065         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21066         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21067         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21068         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21069         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21070         assertNull(calibrator.getEstimatedMa());
21071         assertNull(calibrator.getEstimatedSx());
21072         assertNull(calibrator.getEstimatedSy());
21073         assertNull(calibrator.getEstimatedSz());
21074         assertNull(calibrator.getEstimatedMxy());
21075         assertNull(calibrator.getEstimatedMxz());
21076         assertNull(calibrator.getEstimatedMyx());
21077         assertNull(calibrator.getEstimatedMyz());
21078         assertNull(calibrator.getEstimatedMzx());
21079         assertNull(calibrator.getEstimatedMzy());
21080         assertNull(calibrator.getEstimatedCovariance());
21081         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21082         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21083                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21084         assertNotNull(calibrator.getGroundTruthGravityNorm());
21085         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21086         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21087         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21088                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21089         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21090         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21091         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21092     }
21093 
21094     @Test
21095     public void testConstructor166() throws WrongSizeException {
21096         final Matrix ba = generateBa();
21097         final double biasX = ba.getElementAtIndex(0);
21098         final double biasY = ba.getElementAtIndex(1);
21099         final double biasZ = ba.getElementAtIndex(2);
21100 
21101         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21102         final double latitude = Math.toRadians(
21103                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21104         final double longitude = Math.toRadians(
21105                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21106         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21107         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21108         final NEDVelocity nedVelocity = new NEDVelocity();
21109         final ECEFPosition ecefPosition = new ECEFPosition();
21110         final ECEFVelocity ecefVelocity = new ECEFVelocity();
21111         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21112                 ecefPosition, ecefVelocity);
21113 
21114         final KnownPositionAccelerometerCalibrator calibrator =
21115                 new KnownPositionAccelerometerCalibrator(nedPosition,
21116                         biasX, biasY, biasZ, this);
21117 
21118         // check default values
21119         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21120         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21121         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21122         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21123         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21124         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21125         final Acceleration bx2 = new Acceleration(0.0,
21126                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21127         calibrator.getInitialBiasXAsAcceleration(bx2);
21128         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21129         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21130         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21131         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21132         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21133         final Acceleration by2 = new Acceleration(0.0,
21134                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21135         calibrator.getInitialBiasYAsAcceleration(by2);
21136         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21137         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21138         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21139         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21140         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21141         final Acceleration bz2 = new Acceleration(0.0,
21142                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21143         calibrator.getInitialBiasZAsAcceleration(bz2);
21144         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21145         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21146         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21147         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21148         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21149         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21150         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21151         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21152         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21153         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21154         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21155         final double[] bias1 = calibrator.getInitialBias();
21156         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21157         final double[] bias2 = new double[3];
21158         calibrator.getInitialBias(bias2);
21159         assertArrayEquals(bias1, bias2, 0.0);
21160         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21161         assertEquals(b1, ba);
21162         final Matrix b2 = new Matrix(3, 1);
21163         calibrator.getInitialBiasAsMatrix(b2);
21164         assertEquals(b1, b2);
21165         final Matrix ma1 = calibrator.getInitialMa();
21166         assertEquals(ma1, new Matrix(3, 3));
21167         final Matrix ma2 = new Matrix(3, 3);
21168         calibrator.getInitialMa(ma2);
21169         assertEquals(ma1, ma2);
21170         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21171         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21172         final NEDPosition nedPosition1 = new NEDPosition();
21173         assertTrue(calibrator.getNedPosition(nedPosition1));
21174         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21175         assertNull(calibrator.getMeasurements());
21176         assertFalse(calibrator.isCommonAxisUsed());
21177         assertSame(calibrator.getListener(), this);
21178         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
21179         assertFalse(calibrator.isReady());
21180         assertFalse(calibrator.isRunning());
21181         assertNull(calibrator.getEstimatedBiases());
21182         assertFalse(calibrator.getEstimatedBiases(null));
21183         assertNull(calibrator.getEstimatedBiasesAsMatrix());
21184         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21185         assertNull(calibrator.getEstimatedBiasFx());
21186         assertNull(calibrator.getEstimatedBiasFy());
21187         assertNull(calibrator.getEstimatedBiasFz());
21188         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21189         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21190         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21191         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21192         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21193         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21194         assertNull(calibrator.getEstimatedMa());
21195         assertNull(calibrator.getEstimatedSx());
21196         assertNull(calibrator.getEstimatedSy());
21197         assertNull(calibrator.getEstimatedSz());
21198         assertNull(calibrator.getEstimatedMxy());
21199         assertNull(calibrator.getEstimatedMxz());
21200         assertNull(calibrator.getEstimatedMyx());
21201         assertNull(calibrator.getEstimatedMyz());
21202         assertNull(calibrator.getEstimatedMzx());
21203         assertNull(calibrator.getEstimatedMzy());
21204         assertNull(calibrator.getEstimatedCovariance());
21205         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21206         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21207                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21208         assertNotNull(calibrator.getGroundTruthGravityNorm());
21209         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21210         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21211         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21212                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21213         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21214         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21215         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21216     }
21217 
21218     @Test
21219     public void testConstructor167() throws WrongSizeException {
21220         final Collection<StandardDeviationBodyKinematics> measurements =
21221                 Collections.emptyList();
21222 
21223         final Matrix ba = generateBa();
21224         final double biasX = ba.getElementAtIndex(0);
21225         final double biasY = ba.getElementAtIndex(1);
21226         final double biasZ = ba.getElementAtIndex(2);
21227 
21228         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21229         final double latitude = Math.toRadians(
21230                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21231         final double longitude = Math.toRadians(
21232                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21233         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21234         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21235         final NEDVelocity nedVelocity = new NEDVelocity();
21236         final ECEFPosition ecefPosition = new ECEFPosition();
21237         final ECEFVelocity ecefVelocity = new ECEFVelocity();
21238         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21239                 ecefPosition, ecefVelocity);
21240 
21241         final KnownPositionAccelerometerCalibrator calibrator =
21242                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
21243                         biasX, biasY, biasZ);
21244 
21245         // check default values
21246         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21247         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21248         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21249         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21250         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21251         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21252         final Acceleration bx2 = new Acceleration(0.0,
21253                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21254         calibrator.getInitialBiasXAsAcceleration(bx2);
21255         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21256         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21257         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21258         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21259         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21260         final Acceleration by2 = new Acceleration(0.0,
21261                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21262         calibrator.getInitialBiasYAsAcceleration(by2);
21263         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21264         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21265         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21266         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21267         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21268         final Acceleration bz2 = new Acceleration(0.0,
21269                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21270         calibrator.getInitialBiasZAsAcceleration(bz2);
21271         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21272         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21273         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21274         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21275         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21276         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21277         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21278         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21279         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21280         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21281         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21282         final double[] bias1 = calibrator.getInitialBias();
21283         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21284         final double[] bias2 = new double[3];
21285         calibrator.getInitialBias(bias2);
21286         assertArrayEquals(bias1, bias2, 0.0);
21287         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21288         assertEquals(b1, ba);
21289         final Matrix b2 = new Matrix(3, 1);
21290         calibrator.getInitialBiasAsMatrix(b2);
21291         assertEquals(b1, b2);
21292         final Matrix ma1 = calibrator.getInitialMa();
21293         assertEquals(ma1, new Matrix(3, 3));
21294         final Matrix ma2 = new Matrix(3, 3);
21295         calibrator.getInitialMa(ma2);
21296         assertEquals(ma1, ma2);
21297         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21298         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21299         final NEDPosition nedPosition1 = new NEDPosition();
21300         assertTrue(calibrator.getNedPosition(nedPosition1));
21301         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21302         assertSame(calibrator.getMeasurements(), measurements);
21303         assertFalse(calibrator.isCommonAxisUsed());
21304         assertNull(calibrator.getListener());
21305         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
21306         assertFalse(calibrator.isReady());
21307         assertFalse(calibrator.isRunning());
21308         assertNull(calibrator.getEstimatedBiases());
21309         assertFalse(calibrator.getEstimatedBiases(null));
21310         assertNull(calibrator.getEstimatedBiasesAsMatrix());
21311         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21312         assertNull(calibrator.getEstimatedBiasFx());
21313         assertNull(calibrator.getEstimatedBiasFy());
21314         assertNull(calibrator.getEstimatedBiasFz());
21315         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21316         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21317         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21318         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21319         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21320         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21321         assertNull(calibrator.getEstimatedMa());
21322         assertNull(calibrator.getEstimatedSx());
21323         assertNull(calibrator.getEstimatedSy());
21324         assertNull(calibrator.getEstimatedSz());
21325         assertNull(calibrator.getEstimatedMxy());
21326         assertNull(calibrator.getEstimatedMxz());
21327         assertNull(calibrator.getEstimatedMyx());
21328         assertNull(calibrator.getEstimatedMyz());
21329         assertNull(calibrator.getEstimatedMzx());
21330         assertNull(calibrator.getEstimatedMzy());
21331         assertNull(calibrator.getEstimatedCovariance());
21332         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21333         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21334                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21335         assertNotNull(calibrator.getGroundTruthGravityNorm());
21336         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21337         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21338         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21339                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21340         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21341         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21342         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21343     }
21344 
21345     @Test
21346     public void testConstructor168() throws WrongSizeException {
21347         final Collection<StandardDeviationBodyKinematics> measurements =
21348                 Collections.emptyList();
21349 
21350         final Matrix ba = generateBa();
21351         final double biasX = ba.getElementAtIndex(0);
21352         final double biasY = ba.getElementAtIndex(1);
21353         final double biasZ = ba.getElementAtIndex(2);
21354 
21355         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21356         final double latitude = Math.toRadians(
21357                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21358         final double longitude = Math.toRadians(
21359                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21360         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21361         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21362         final NEDVelocity nedVelocity = new NEDVelocity();
21363         final ECEFPosition ecefPosition = new ECEFPosition();
21364         final ECEFVelocity ecefVelocity = new ECEFVelocity();
21365         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21366                 ecefPosition, ecefVelocity);
21367 
21368         final KnownPositionAccelerometerCalibrator calibrator =
21369                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
21370                         biasX, biasY, biasZ, this);
21371 
21372         // check default values
21373         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21374         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21375         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21376         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21377         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21378         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21379         final Acceleration bx2 = new Acceleration(0.0,
21380                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21381         calibrator.getInitialBiasXAsAcceleration(bx2);
21382         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21383         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21384         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21385         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21386         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21387         final Acceleration by2 = new Acceleration(0.0,
21388                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21389         calibrator.getInitialBiasYAsAcceleration(by2);
21390         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21391         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21392         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21393         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21394         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21395         final Acceleration bz2 = new Acceleration(0.0,
21396                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21397         calibrator.getInitialBiasZAsAcceleration(bz2);
21398         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21399         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21400         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21401         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21402         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21403         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21404         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21405         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21406         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21407         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21408         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21409         final double[] bias1 = calibrator.getInitialBias();
21410         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21411         final double[] bias2 = new double[3];
21412         calibrator.getInitialBias(bias2);
21413         assertArrayEquals(bias1, bias2, 0.0);
21414         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21415         assertEquals(b1, ba);
21416         final Matrix b2 = new Matrix(3, 1);
21417         calibrator.getInitialBiasAsMatrix(b2);
21418         assertEquals(b1, b2);
21419         final Matrix ma1 = calibrator.getInitialMa();
21420         assertEquals(ma1, new Matrix(3, 3));
21421         final Matrix ma2 = new Matrix(3, 3);
21422         calibrator.getInitialMa(ma2);
21423         assertEquals(ma1, ma2);
21424         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21425         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21426         final NEDPosition nedPosition1 = new NEDPosition();
21427         assertTrue(calibrator.getNedPosition(nedPosition1));
21428         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21429         assertSame(calibrator.getMeasurements(), measurements);
21430         assertFalse(calibrator.isCommonAxisUsed());
21431         assertSame(calibrator.getListener(), this);
21432         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
21433         assertFalse(calibrator.isReady());
21434         assertFalse(calibrator.isRunning());
21435         assertNull(calibrator.getEstimatedBiases());
21436         assertFalse(calibrator.getEstimatedBiases(null));
21437         assertNull(calibrator.getEstimatedBiasesAsMatrix());
21438         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21439         assertNull(calibrator.getEstimatedBiasFx());
21440         assertNull(calibrator.getEstimatedBiasFy());
21441         assertNull(calibrator.getEstimatedBiasFz());
21442         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21443         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21444         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21445         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21446         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21447         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21448         assertNull(calibrator.getEstimatedMa());
21449         assertNull(calibrator.getEstimatedSx());
21450         assertNull(calibrator.getEstimatedSy());
21451         assertNull(calibrator.getEstimatedSz());
21452         assertNull(calibrator.getEstimatedMxy());
21453         assertNull(calibrator.getEstimatedMxz());
21454         assertNull(calibrator.getEstimatedMyx());
21455         assertNull(calibrator.getEstimatedMyz());
21456         assertNull(calibrator.getEstimatedMzx());
21457         assertNull(calibrator.getEstimatedMzy());
21458         assertNull(calibrator.getEstimatedCovariance());
21459         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21460         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21461                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21462         assertNotNull(calibrator.getGroundTruthGravityNorm());
21463         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21464         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21465         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21466                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21467         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21468         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21469         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21470     }
21471 
21472     @Test
21473     public void testConstructor169() throws WrongSizeException {
21474         final Matrix ba = generateBa();
21475         final double biasX = ba.getElementAtIndex(0);
21476         final double biasY = ba.getElementAtIndex(1);
21477         final double biasZ = ba.getElementAtIndex(2);
21478 
21479         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21480         final double latitude = Math.toRadians(
21481                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21482         final double longitude = Math.toRadians(
21483                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21484         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21485         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21486         final NEDVelocity nedVelocity = new NEDVelocity();
21487         final ECEFPosition ecefPosition = new ECEFPosition();
21488         final ECEFVelocity ecefVelocity = new ECEFVelocity();
21489         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21490                 ecefPosition, ecefVelocity);
21491 
21492         final KnownPositionAccelerometerCalibrator calibrator =
21493                 new KnownPositionAccelerometerCalibrator(nedPosition,
21494                         true, biasX, biasY, biasZ);
21495 
21496         // check default values
21497         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21498         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21499         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21500         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21501         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21502         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21503         final Acceleration bx2 = new Acceleration(0.0,
21504                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21505         calibrator.getInitialBiasXAsAcceleration(bx2);
21506         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21507         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21508         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21509         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21510         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21511         final Acceleration by2 = new Acceleration(0.0,
21512                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21513         calibrator.getInitialBiasYAsAcceleration(by2);
21514         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21515         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21516         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21517         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21518         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21519         final Acceleration bz2 = new Acceleration(0.0,
21520                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21521         calibrator.getInitialBiasZAsAcceleration(bz2);
21522         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21523         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21524         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21525         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21526         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21527         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21528         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21529         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21530         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21531         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21532         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21533         final double[] bias1 = calibrator.getInitialBias();
21534         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21535         final double[] bias2 = new double[3];
21536         calibrator.getInitialBias(bias2);
21537         assertArrayEquals(bias1, bias2, 0.0);
21538         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21539         assertEquals(b1, ba);
21540         final Matrix b2 = new Matrix(3, 1);
21541         calibrator.getInitialBiasAsMatrix(b2);
21542         assertEquals(b1, b2);
21543         final Matrix ma1 = calibrator.getInitialMa();
21544         assertEquals(ma1, new Matrix(3, 3));
21545         final Matrix ma2 = new Matrix(3, 3);
21546         calibrator.getInitialMa(ma2);
21547         assertEquals(ma1, ma2);
21548         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21549         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21550         final NEDPosition nedPosition1 = new NEDPosition();
21551         assertTrue(calibrator.getNedPosition(nedPosition1));
21552         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21553         assertNull(calibrator.getMeasurements());
21554         assertTrue(calibrator.isCommonAxisUsed());
21555         assertNull(calibrator.getListener());
21556         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
21557         assertFalse(calibrator.isReady());
21558         assertFalse(calibrator.isRunning());
21559         assertNull(calibrator.getEstimatedBiases());
21560         assertFalse(calibrator.getEstimatedBiases(null));
21561         assertNull(calibrator.getEstimatedBiasesAsMatrix());
21562         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21563         assertNull(calibrator.getEstimatedBiasFx());
21564         assertNull(calibrator.getEstimatedBiasFy());
21565         assertNull(calibrator.getEstimatedBiasFz());
21566         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21567         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21568         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21569         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21570         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21571         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21572         assertNull(calibrator.getEstimatedMa());
21573         assertNull(calibrator.getEstimatedSx());
21574         assertNull(calibrator.getEstimatedSy());
21575         assertNull(calibrator.getEstimatedSz());
21576         assertNull(calibrator.getEstimatedMxy());
21577         assertNull(calibrator.getEstimatedMxz());
21578         assertNull(calibrator.getEstimatedMyx());
21579         assertNull(calibrator.getEstimatedMyz());
21580         assertNull(calibrator.getEstimatedMzx());
21581         assertNull(calibrator.getEstimatedMzy());
21582         assertNull(calibrator.getEstimatedCovariance());
21583         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21584         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21585                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21586         assertNotNull(calibrator.getGroundTruthGravityNorm());
21587         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21588         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21589         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21590                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21591         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21592         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21593         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21594     }
21595 
21596     @Test
21597     public void testConstructor170() throws WrongSizeException {
21598         final Matrix ba = generateBa();
21599         final double biasX = ba.getElementAtIndex(0);
21600         final double biasY = ba.getElementAtIndex(1);
21601         final double biasZ = ba.getElementAtIndex(2);
21602 
21603         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21604         final double latitude = Math.toRadians(
21605                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21606         final double longitude = Math.toRadians(
21607                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21608         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21609         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21610         final NEDVelocity nedVelocity = new NEDVelocity();
21611         final ECEFPosition ecefPosition = new ECEFPosition();
21612         final ECEFVelocity ecefVelocity = new ECEFVelocity();
21613         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21614                 ecefPosition, ecefVelocity);
21615 
21616         final KnownPositionAccelerometerCalibrator calibrator =
21617                 new KnownPositionAccelerometerCalibrator(nedPosition,
21618                         true, biasX, biasY, biasZ, this);
21619 
21620         // check default values
21621         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21622         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21623         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21624         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21625         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21626         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21627         final Acceleration bx2 = new Acceleration(0.0,
21628                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21629         calibrator.getInitialBiasXAsAcceleration(bx2);
21630         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21631         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21632         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21633         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21634         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21635         final Acceleration by2 = new Acceleration(0.0,
21636                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21637         calibrator.getInitialBiasYAsAcceleration(by2);
21638         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21639         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21640         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21641         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21642         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21643         final Acceleration bz2 = new Acceleration(0.0,
21644                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21645         calibrator.getInitialBiasZAsAcceleration(bz2);
21646         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21647         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21648         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21649         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21650         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21651         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21652         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21653         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21654         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21655         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21656         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21657         final double[] bias1 = calibrator.getInitialBias();
21658         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21659         final double[] bias2 = new double[3];
21660         calibrator.getInitialBias(bias2);
21661         assertArrayEquals(bias1, bias2, 0.0);
21662         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21663         assertEquals(b1, ba);
21664         final Matrix b2 = new Matrix(3, 1);
21665         calibrator.getInitialBiasAsMatrix(b2);
21666         assertEquals(b1, b2);
21667         final Matrix ma1 = calibrator.getInitialMa();
21668         assertEquals(ma1, new Matrix(3, 3));
21669         final Matrix ma2 = new Matrix(3, 3);
21670         calibrator.getInitialMa(ma2);
21671         assertEquals(ma1, ma2);
21672         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21673         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21674         final NEDPosition nedPosition1 = new NEDPosition();
21675         assertTrue(calibrator.getNedPosition(nedPosition1));
21676         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21677         assertNull(calibrator.getMeasurements());
21678         assertTrue(calibrator.isCommonAxisUsed());
21679         assertSame(calibrator.getListener(), this);
21680         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
21681         assertFalse(calibrator.isReady());
21682         assertFalse(calibrator.isRunning());
21683         assertNull(calibrator.getEstimatedBiases());
21684         assertFalse(calibrator.getEstimatedBiases(null));
21685         assertNull(calibrator.getEstimatedBiasesAsMatrix());
21686         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21687         assertNull(calibrator.getEstimatedBiasFx());
21688         assertNull(calibrator.getEstimatedBiasFy());
21689         assertNull(calibrator.getEstimatedBiasFz());
21690         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21691         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21692         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21693         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21694         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21695         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21696         assertNull(calibrator.getEstimatedMa());
21697         assertNull(calibrator.getEstimatedSx());
21698         assertNull(calibrator.getEstimatedSy());
21699         assertNull(calibrator.getEstimatedSz());
21700         assertNull(calibrator.getEstimatedMxy());
21701         assertNull(calibrator.getEstimatedMxz());
21702         assertNull(calibrator.getEstimatedMyx());
21703         assertNull(calibrator.getEstimatedMyz());
21704         assertNull(calibrator.getEstimatedMzx());
21705         assertNull(calibrator.getEstimatedMzy());
21706         assertNull(calibrator.getEstimatedCovariance());
21707         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21708         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21709                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21710         assertNotNull(calibrator.getGroundTruthGravityNorm());
21711         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21712         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21713         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21714                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21715         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21716         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21717         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21718     }
21719 
21720     @Test
21721     public void testConstructor171() throws WrongSizeException {
21722         final Collection<StandardDeviationBodyKinematics> measurements =
21723                 Collections.emptyList();
21724 
21725         final Matrix ba = generateBa();
21726         final double biasX = ba.getElementAtIndex(0);
21727         final double biasY = ba.getElementAtIndex(1);
21728         final double biasZ = ba.getElementAtIndex(2);
21729 
21730         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21731         final double latitude = Math.toRadians(
21732                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21733         final double longitude = Math.toRadians(
21734                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21735         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21736         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21737         final NEDVelocity nedVelocity = new NEDVelocity();
21738         final ECEFPosition ecefPosition = new ECEFPosition();
21739         final ECEFVelocity ecefVelocity = new ECEFVelocity();
21740         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21741                 ecefPosition, ecefVelocity);
21742 
21743         final KnownPositionAccelerometerCalibrator calibrator =
21744                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
21745                         true, biasX, biasY, biasZ);
21746 
21747         // check default values
21748         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21749         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21750         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21751         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21752         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21753         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21754         final Acceleration bx2 = new Acceleration(0.0,
21755                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21756         calibrator.getInitialBiasXAsAcceleration(bx2);
21757         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21758         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21759         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21760         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21761         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21762         final Acceleration by2 = new Acceleration(0.0,
21763                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21764         calibrator.getInitialBiasYAsAcceleration(by2);
21765         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21766         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21767         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21768         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21769         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21770         final Acceleration bz2 = new Acceleration(0.0,
21771                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21772         calibrator.getInitialBiasZAsAcceleration(bz2);
21773         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21774         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21775         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21776         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21777         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21778         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21779         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21780         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21781         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21782         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21783         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21784         final double[] bias1 = calibrator.getInitialBias();
21785         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21786         final double[] bias2 = new double[3];
21787         calibrator.getInitialBias(bias2);
21788         assertArrayEquals(bias1, bias2, 0.0);
21789         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21790         assertEquals(b1, ba);
21791         final Matrix b2 = new Matrix(3, 1);
21792         calibrator.getInitialBiasAsMatrix(b2);
21793         assertEquals(b1, b2);
21794         final Matrix ma1 = calibrator.getInitialMa();
21795         assertEquals(ma1, new Matrix(3, 3));
21796         final Matrix ma2 = new Matrix(3, 3);
21797         calibrator.getInitialMa(ma2);
21798         assertEquals(ma1, ma2);
21799         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21800         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21801         final NEDPosition nedPosition1 = new NEDPosition();
21802         assertTrue(calibrator.getNedPosition(nedPosition1));
21803         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21804         assertSame(calibrator.getMeasurements(), measurements);
21805         assertTrue(calibrator.isCommonAxisUsed());
21806         assertNull(calibrator.getListener());
21807         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
21808         assertFalse(calibrator.isReady());
21809         assertFalse(calibrator.isRunning());
21810         assertNull(calibrator.getEstimatedBiases());
21811         assertFalse(calibrator.getEstimatedBiases(null));
21812         assertNull(calibrator.getEstimatedBiasesAsMatrix());
21813         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21814         assertNull(calibrator.getEstimatedBiasFx());
21815         assertNull(calibrator.getEstimatedBiasFy());
21816         assertNull(calibrator.getEstimatedBiasFz());
21817         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21818         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21819         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21820         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21821         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21822         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21823         assertNull(calibrator.getEstimatedMa());
21824         assertNull(calibrator.getEstimatedSx());
21825         assertNull(calibrator.getEstimatedSy());
21826         assertNull(calibrator.getEstimatedSz());
21827         assertNull(calibrator.getEstimatedMxy());
21828         assertNull(calibrator.getEstimatedMxz());
21829         assertNull(calibrator.getEstimatedMyx());
21830         assertNull(calibrator.getEstimatedMyz());
21831         assertNull(calibrator.getEstimatedMzx());
21832         assertNull(calibrator.getEstimatedMzy());
21833         assertNull(calibrator.getEstimatedCovariance());
21834         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21835         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21836                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21837         assertNotNull(calibrator.getGroundTruthGravityNorm());
21838         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21839         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21840         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21841                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21842         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21843         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21844         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21845     }
21846 
21847     @Test
21848     public void testConstructor172() throws WrongSizeException {
21849         final Collection<StandardDeviationBodyKinematics> measurements =
21850                 Collections.emptyList();
21851 
21852         final Matrix ba = generateBa();
21853         final double biasX = ba.getElementAtIndex(0);
21854         final double biasY = ba.getElementAtIndex(1);
21855         final double biasZ = ba.getElementAtIndex(2);
21856 
21857         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21858         final double latitude = Math.toRadians(
21859                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21860         final double longitude = Math.toRadians(
21861                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21862         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21863         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21864         final NEDVelocity nedVelocity = new NEDVelocity();
21865         final ECEFPosition ecefPosition = new ECEFPosition();
21866         final ECEFVelocity ecefVelocity = new ECEFVelocity();
21867         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21868                 ecefPosition, ecefVelocity);
21869 
21870         final KnownPositionAccelerometerCalibrator calibrator =
21871                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
21872                         true, biasX, biasY, biasZ, this);
21873 
21874         // check default values
21875         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21876         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21877         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21878         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21879         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21880         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21881         final Acceleration bx2 = new Acceleration(0.0,
21882                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21883         calibrator.getInitialBiasXAsAcceleration(bx2);
21884         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21885         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21886         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21887         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21888         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21889         final Acceleration by2 = new Acceleration(0.0,
21890                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21891         calibrator.getInitialBiasYAsAcceleration(by2);
21892         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21893         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21894         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21895         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21896         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21897         final Acceleration bz2 = new Acceleration(0.0,
21898                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21899         calibrator.getInitialBiasZAsAcceleration(bz2);
21900         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21901         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21902         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21903         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21904         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21905         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21906         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21907         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21908         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21909         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21910         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21911         final double[] bias1 = calibrator.getInitialBias();
21912         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21913         final double[] bias2 = new double[3];
21914         calibrator.getInitialBias(bias2);
21915         assertArrayEquals(bias1, bias2, 0.0);
21916         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21917         assertEquals(b1, ba);
21918         final Matrix b2 = new Matrix(3, 1);
21919         calibrator.getInitialBiasAsMatrix(b2);
21920         assertEquals(b1, b2);
21921         final Matrix ma1 = calibrator.getInitialMa();
21922         assertEquals(ma1, new Matrix(3, 3));
21923         final Matrix ma2 = new Matrix(3, 3);
21924         calibrator.getInitialMa(ma2);
21925         assertEquals(ma1, ma2);
21926         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21927         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21928         final NEDPosition nedPosition1 = new NEDPosition();
21929         assertTrue(calibrator.getNedPosition(nedPosition1));
21930         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21931         assertSame(calibrator.getMeasurements(), measurements);
21932         assertTrue(calibrator.isCommonAxisUsed());
21933         assertSame(calibrator.getListener(), this);
21934         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
21935         assertFalse(calibrator.isReady());
21936         assertFalse(calibrator.isRunning());
21937         assertNull(calibrator.getEstimatedBiases());
21938         assertFalse(calibrator.getEstimatedBiases(null));
21939         assertNull(calibrator.getEstimatedBiasesAsMatrix());
21940         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21941         assertNull(calibrator.getEstimatedBiasFx());
21942         assertNull(calibrator.getEstimatedBiasFy());
21943         assertNull(calibrator.getEstimatedBiasFz());
21944         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21945         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21946         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21947         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21948         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21949         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21950         assertNull(calibrator.getEstimatedMa());
21951         assertNull(calibrator.getEstimatedSx());
21952         assertNull(calibrator.getEstimatedSy());
21953         assertNull(calibrator.getEstimatedSz());
21954         assertNull(calibrator.getEstimatedMxy());
21955         assertNull(calibrator.getEstimatedMxz());
21956         assertNull(calibrator.getEstimatedMyx());
21957         assertNull(calibrator.getEstimatedMyz());
21958         assertNull(calibrator.getEstimatedMzx());
21959         assertNull(calibrator.getEstimatedMzy());
21960         assertNull(calibrator.getEstimatedCovariance());
21961         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21962         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21963                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21964         assertNotNull(calibrator.getGroundTruthGravityNorm());
21965         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21966         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21967         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21968                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21969         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21970         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21971         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21972     }
21973 
21974     @Test
21975     public void testConstructor173() throws WrongSizeException {
21976         final Matrix ba = generateBa();
21977         final double biasX = ba.getElementAtIndex(0);
21978         final double biasY = ba.getElementAtIndex(1);
21979         final double biasZ = ba.getElementAtIndex(2);
21980 
21981         final Acceleration bx = new Acceleration(biasX,
21982                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21983         final Acceleration by = new Acceleration(biasY,
21984                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21985         final Acceleration bz = new Acceleration(biasZ,
21986                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21987 
21988         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21989         final double latitude = Math.toRadians(
21990                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21991         final double longitude = Math.toRadians(
21992                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21993         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21994         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21995         final NEDVelocity nedVelocity = new NEDVelocity();
21996         final ECEFPosition ecefPosition = new ECEFPosition();
21997         final ECEFVelocity ecefVelocity = new ECEFVelocity();
21998         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21999                 ecefPosition, ecefVelocity);
22000 
22001         final KnownPositionAccelerometerCalibrator calibrator =
22002                 new KnownPositionAccelerometerCalibrator(nedPosition,
22003                         bx, by, bz);
22004 
22005         // check default values
22006         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22007         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22008         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22009         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22010         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22011         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22012         final Acceleration bx2 = new Acceleration(0.0,
22013                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22014         calibrator.getInitialBiasXAsAcceleration(bx2);
22015         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22016         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22017         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22018         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22019         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22020         final Acceleration by2 = new Acceleration(0.0,
22021                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22022         calibrator.getInitialBiasYAsAcceleration(by2);
22023         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22024         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22025         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22026         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22027         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22028         final Acceleration bz2 = new Acceleration(0.0,
22029                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22030         calibrator.getInitialBiasZAsAcceleration(bz2);
22031         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22032         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22033         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22034         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22035         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22036         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22037         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22038         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22039         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22040         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22041         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22042         final double[] bias1 = calibrator.getInitialBias();
22043         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22044         final double[] bias2 = new double[3];
22045         calibrator.getInitialBias(bias2);
22046         assertArrayEquals(bias1, bias2, 0.0);
22047         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22048         assertEquals(b1, ba);
22049         final Matrix b2 = new Matrix(3, 1);
22050         calibrator.getInitialBiasAsMatrix(b2);
22051         assertEquals(b1, b2);
22052         final Matrix ma1 = calibrator.getInitialMa();
22053         assertEquals(ma1, new Matrix(3, 3));
22054         final Matrix ma2 = new Matrix(3, 3);
22055         calibrator.getInitialMa(ma2);
22056         assertEquals(ma1, ma2);
22057         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22058         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22059         final NEDPosition nedPosition1 = new NEDPosition();
22060         assertTrue(calibrator.getNedPosition(nedPosition1));
22061         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22062         assertNull(calibrator.getMeasurements());
22063         assertFalse(calibrator.isCommonAxisUsed());
22064         assertNull(calibrator.getListener());
22065         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
22066         assertFalse(calibrator.isReady());
22067         assertFalse(calibrator.isRunning());
22068         assertNull(calibrator.getEstimatedBiases());
22069         assertFalse(calibrator.getEstimatedBiases(null));
22070         assertNull(calibrator.getEstimatedBiasesAsMatrix());
22071         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22072         assertNull(calibrator.getEstimatedBiasFx());
22073         assertNull(calibrator.getEstimatedBiasFy());
22074         assertNull(calibrator.getEstimatedBiasFz());
22075         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22076         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22077         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22078         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22079         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22080         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22081         assertNull(calibrator.getEstimatedMa());
22082         assertNull(calibrator.getEstimatedSx());
22083         assertNull(calibrator.getEstimatedSy());
22084         assertNull(calibrator.getEstimatedSz());
22085         assertNull(calibrator.getEstimatedMxy());
22086         assertNull(calibrator.getEstimatedMxz());
22087         assertNull(calibrator.getEstimatedMyx());
22088         assertNull(calibrator.getEstimatedMyz());
22089         assertNull(calibrator.getEstimatedMzx());
22090         assertNull(calibrator.getEstimatedMzy());
22091         assertNull(calibrator.getEstimatedCovariance());
22092         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22093         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22094                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22095         assertNotNull(calibrator.getGroundTruthGravityNorm());
22096         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22097         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22098         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22099                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22100         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22101         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22102         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22103     }
22104 
22105     @Test
22106     public void testConstructor174() throws WrongSizeException {
22107         final Matrix ba = generateBa();
22108         final double biasX = ba.getElementAtIndex(0);
22109         final double biasY = ba.getElementAtIndex(1);
22110         final double biasZ = ba.getElementAtIndex(2);
22111 
22112         final Acceleration bx = new Acceleration(biasX,
22113                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22114         final Acceleration by = new Acceleration(biasY,
22115                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22116         final Acceleration bz = new Acceleration(biasZ,
22117                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22118 
22119         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22120         final double latitude = Math.toRadians(
22121                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22122         final double longitude = Math.toRadians(
22123                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22124         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22125         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22126         final NEDVelocity nedVelocity = new NEDVelocity();
22127         final ECEFPosition ecefPosition = new ECEFPosition();
22128         final ECEFVelocity ecefVelocity = new ECEFVelocity();
22129         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22130                 ecefPosition, ecefVelocity);
22131 
22132         final KnownPositionAccelerometerCalibrator calibrator =
22133                 new KnownPositionAccelerometerCalibrator(nedPosition,
22134                         bx, by, bz, this);
22135 
22136         // check default values
22137         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22138         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22139         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22140         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22141         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22142         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22143         final Acceleration bx2 = new Acceleration(0.0,
22144                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22145         calibrator.getInitialBiasXAsAcceleration(bx2);
22146         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22147         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22148         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22149         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22150         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22151         final Acceleration by2 = new Acceleration(0.0,
22152                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22153         calibrator.getInitialBiasYAsAcceleration(by2);
22154         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22155         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22156         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22157         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22158         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22159         final Acceleration bz2 = new Acceleration(0.0,
22160                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22161         calibrator.getInitialBiasZAsAcceleration(bz2);
22162         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22163         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22164         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22165         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22166         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22167         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22168         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22169         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22170         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22171         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22172         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22173         final double[] bias1 = calibrator.getInitialBias();
22174         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22175         final double[] bias2 = new double[3];
22176         calibrator.getInitialBias(bias2);
22177         assertArrayEquals(bias1, bias2, 0.0);
22178         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22179         assertEquals(b1, ba);
22180         final Matrix b2 = new Matrix(3, 1);
22181         calibrator.getInitialBiasAsMatrix(b2);
22182         assertEquals(b1, b2);
22183         final Matrix ma1 = calibrator.getInitialMa();
22184         assertEquals(ma1, new Matrix(3, 3));
22185         final Matrix ma2 = new Matrix(3, 3);
22186         calibrator.getInitialMa(ma2);
22187         assertEquals(ma1, ma2);
22188         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22189         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22190         final NEDPosition nedPosition1 = new NEDPosition();
22191         assertTrue(calibrator.getNedPosition(nedPosition1));
22192         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22193         assertNull(calibrator.getMeasurements());
22194         assertFalse(calibrator.isCommonAxisUsed());
22195         assertSame(calibrator.getListener(), this);
22196         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
22197         assertFalse(calibrator.isReady());
22198         assertFalse(calibrator.isRunning());
22199         assertNull(calibrator.getEstimatedBiases());
22200         assertFalse(calibrator.getEstimatedBiases(null));
22201         assertNull(calibrator.getEstimatedBiasesAsMatrix());
22202         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22203         assertNull(calibrator.getEstimatedBiasFx());
22204         assertNull(calibrator.getEstimatedBiasFy());
22205         assertNull(calibrator.getEstimatedBiasFz());
22206         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22207         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22208         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22209         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22210         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22211         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22212         assertNull(calibrator.getEstimatedMa());
22213         assertNull(calibrator.getEstimatedSx());
22214         assertNull(calibrator.getEstimatedSy());
22215         assertNull(calibrator.getEstimatedSz());
22216         assertNull(calibrator.getEstimatedMxy());
22217         assertNull(calibrator.getEstimatedMxz());
22218         assertNull(calibrator.getEstimatedMyx());
22219         assertNull(calibrator.getEstimatedMyz());
22220         assertNull(calibrator.getEstimatedMzx());
22221         assertNull(calibrator.getEstimatedMzy());
22222         assertNull(calibrator.getEstimatedCovariance());
22223         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22224         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22225                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22226         assertNotNull(calibrator.getGroundTruthGravityNorm());
22227         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22228         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22229         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22230                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22231         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22232         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22233         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22234     }
22235 
22236     @Test
22237     public void testConstructor175() throws WrongSizeException {
22238         final Collection<StandardDeviationBodyKinematics> measurements =
22239                 Collections.emptyList();
22240 
22241         final Matrix ba = generateBa();
22242         final double biasX = ba.getElementAtIndex(0);
22243         final double biasY = ba.getElementAtIndex(1);
22244         final double biasZ = ba.getElementAtIndex(2);
22245 
22246         final Acceleration bx = new Acceleration(biasX,
22247                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22248         final Acceleration by = new Acceleration(biasY,
22249                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22250         final Acceleration bz = new Acceleration(biasZ,
22251                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22252 
22253         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22254         final double latitude = Math.toRadians(
22255                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22256         final double longitude = Math.toRadians(
22257                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22258         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22259         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22260         final NEDVelocity nedVelocity = new NEDVelocity();
22261         final ECEFPosition ecefPosition = new ECEFPosition();
22262         final ECEFVelocity ecefVelocity = new ECEFVelocity();
22263         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22264                 ecefPosition, ecefVelocity);
22265 
22266         final KnownPositionAccelerometerCalibrator calibrator =
22267                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
22268                         bx, by, bz);
22269 
22270         // check default values
22271         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22272         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22273         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22274         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22275         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22276         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22277         final Acceleration bx2 = new Acceleration(0.0,
22278                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22279         calibrator.getInitialBiasXAsAcceleration(bx2);
22280         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22281         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22282         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22283         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22284         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22285         final Acceleration by2 = new Acceleration(0.0,
22286                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22287         calibrator.getInitialBiasYAsAcceleration(by2);
22288         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22289         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22290         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22291         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22292         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22293         final Acceleration bz2 = new Acceleration(0.0,
22294                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22295         calibrator.getInitialBiasZAsAcceleration(bz2);
22296         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22297         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22298         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22299         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22300         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22301         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22302         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22303         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22304         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22305         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22306         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22307         final double[] bias1 = calibrator.getInitialBias();
22308         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22309         final double[] bias2 = new double[3];
22310         calibrator.getInitialBias(bias2);
22311         assertArrayEquals(bias1, bias2, 0.0);
22312         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22313         assertEquals(b1, ba);
22314         final Matrix b2 = new Matrix(3, 1);
22315         calibrator.getInitialBiasAsMatrix(b2);
22316         assertEquals(b1, b2);
22317         final Matrix ma1 = calibrator.getInitialMa();
22318         assertEquals(ma1, new Matrix(3, 3));
22319         final Matrix ma2 = new Matrix(3, 3);
22320         calibrator.getInitialMa(ma2);
22321         assertEquals(ma1, ma2);
22322         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22323         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22324         final NEDPosition nedPosition1 = new NEDPosition();
22325         assertTrue(calibrator.getNedPosition(nedPosition1));
22326         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22327         assertSame(calibrator.getMeasurements(), measurements);
22328         assertFalse(calibrator.isCommonAxisUsed());
22329         assertNull(calibrator.getListener());
22330         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
22331         assertFalse(calibrator.isReady());
22332         assertFalse(calibrator.isRunning());
22333         assertNull(calibrator.getEstimatedBiases());
22334         assertFalse(calibrator.getEstimatedBiases(null));
22335         assertNull(calibrator.getEstimatedBiasesAsMatrix());
22336         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22337         assertNull(calibrator.getEstimatedBiasFx());
22338         assertNull(calibrator.getEstimatedBiasFy());
22339         assertNull(calibrator.getEstimatedBiasFz());
22340         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22341         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22342         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22343         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22344         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22345         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22346         assertNull(calibrator.getEstimatedMa());
22347         assertNull(calibrator.getEstimatedSx());
22348         assertNull(calibrator.getEstimatedSy());
22349         assertNull(calibrator.getEstimatedSz());
22350         assertNull(calibrator.getEstimatedMxy());
22351         assertNull(calibrator.getEstimatedMxz());
22352         assertNull(calibrator.getEstimatedMyx());
22353         assertNull(calibrator.getEstimatedMyz());
22354         assertNull(calibrator.getEstimatedMzx());
22355         assertNull(calibrator.getEstimatedMzy());
22356         assertNull(calibrator.getEstimatedCovariance());
22357         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22358         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22359                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22360         assertNotNull(calibrator.getGroundTruthGravityNorm());
22361         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22362         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22363         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22364                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22365         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22366         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22367         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22368     }
22369 
22370     @Test
22371     public void testConstructor176() throws WrongSizeException {
22372         final Collection<StandardDeviationBodyKinematics> measurements =
22373                 Collections.emptyList();
22374 
22375         final Matrix ba = generateBa();
22376         final double biasX = ba.getElementAtIndex(0);
22377         final double biasY = ba.getElementAtIndex(1);
22378         final double biasZ = ba.getElementAtIndex(2);
22379 
22380         final Acceleration bx = new Acceleration(biasX,
22381                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22382         final Acceleration by = new Acceleration(biasY,
22383                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22384         final Acceleration bz = new Acceleration(biasZ,
22385                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22386 
22387         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22388         final double latitude = Math.toRadians(
22389                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22390         final double longitude = Math.toRadians(
22391                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22392         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22393         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22394         final NEDVelocity nedVelocity = new NEDVelocity();
22395         final ECEFPosition ecefPosition = new ECEFPosition();
22396         final ECEFVelocity ecefVelocity = new ECEFVelocity();
22397         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22398                 ecefPosition, ecefVelocity);
22399 
22400         final KnownPositionAccelerometerCalibrator calibrator =
22401                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
22402                         bx, by, bz, this);
22403 
22404         // check default values
22405         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22406         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22407         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22408         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22409         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22410         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22411         final Acceleration bx2 = new Acceleration(0.0,
22412                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22413         calibrator.getInitialBiasXAsAcceleration(bx2);
22414         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22415         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22416         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22417         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22418         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22419         final Acceleration by2 = new Acceleration(0.0,
22420                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22421         calibrator.getInitialBiasYAsAcceleration(by2);
22422         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22423         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22424         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22425         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22426         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22427         final Acceleration bz2 = new Acceleration(0.0,
22428                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22429         calibrator.getInitialBiasZAsAcceleration(bz2);
22430         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22431         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22432         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22433         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22434         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22435         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22436         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22437         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22438         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22439         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22440         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22441         final double[] bias1 = calibrator.getInitialBias();
22442         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22443         final double[] bias2 = new double[3];
22444         calibrator.getInitialBias(bias2);
22445         assertArrayEquals(bias1, bias2, 0.0);
22446         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22447         assertEquals(b1, ba);
22448         final Matrix b2 = new Matrix(3, 1);
22449         calibrator.getInitialBiasAsMatrix(b2);
22450         assertEquals(b1, b2);
22451         final Matrix ma1 = calibrator.getInitialMa();
22452         assertEquals(ma1, new Matrix(3, 3));
22453         final Matrix ma2 = new Matrix(3, 3);
22454         calibrator.getInitialMa(ma2);
22455         assertEquals(ma1, ma2);
22456         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22457         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22458         final NEDPosition nedPosition1 = new NEDPosition();
22459         assertTrue(calibrator.getNedPosition(nedPosition1));
22460         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22461         assertSame(calibrator.getMeasurements(), measurements);
22462         assertFalse(calibrator.isCommonAxisUsed());
22463         assertSame(calibrator.getListener(), this);
22464         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
22465         assertFalse(calibrator.isReady());
22466         assertFalse(calibrator.isRunning());
22467         assertNull(calibrator.getEstimatedBiases());
22468         assertFalse(calibrator.getEstimatedBiases(null));
22469         assertNull(calibrator.getEstimatedBiasesAsMatrix());
22470         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22471         assertNull(calibrator.getEstimatedBiasFx());
22472         assertNull(calibrator.getEstimatedBiasFy());
22473         assertNull(calibrator.getEstimatedBiasFz());
22474         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22475         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22476         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22477         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22478         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22479         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22480         assertNull(calibrator.getEstimatedMa());
22481         assertNull(calibrator.getEstimatedSx());
22482         assertNull(calibrator.getEstimatedSy());
22483         assertNull(calibrator.getEstimatedSz());
22484         assertNull(calibrator.getEstimatedMxy());
22485         assertNull(calibrator.getEstimatedMxz());
22486         assertNull(calibrator.getEstimatedMyx());
22487         assertNull(calibrator.getEstimatedMyz());
22488         assertNull(calibrator.getEstimatedMzx());
22489         assertNull(calibrator.getEstimatedMzy());
22490         assertNull(calibrator.getEstimatedCovariance());
22491         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22492         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22493                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22494         assertNotNull(calibrator.getGroundTruthGravityNorm());
22495         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22496         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22497         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22498                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22499         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22500         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22501         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22502     }
22503 
22504     @Test
22505     public void testConstructor177() throws WrongSizeException {
22506         final Matrix ba = generateBa();
22507         final double biasX = ba.getElementAtIndex(0);
22508         final double biasY = ba.getElementAtIndex(1);
22509         final double biasZ = ba.getElementAtIndex(2);
22510 
22511         final Acceleration bx = new Acceleration(biasX,
22512                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22513         final Acceleration by = new Acceleration(biasY,
22514                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22515         final Acceleration bz = new Acceleration(biasZ,
22516                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22517 
22518         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22519         final double latitude = Math.toRadians(
22520                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22521         final double longitude = Math.toRadians(
22522                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22523         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22524         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22525         final NEDVelocity nedVelocity = new NEDVelocity();
22526         final ECEFPosition ecefPosition = new ECEFPosition();
22527         final ECEFVelocity ecefVelocity = new ECEFVelocity();
22528         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22529                 ecefPosition, ecefVelocity);
22530 
22531         final KnownPositionAccelerometerCalibrator calibrator =
22532                 new KnownPositionAccelerometerCalibrator(nedPosition,
22533                         true, bx, by, bz);
22534 
22535         // check default values
22536         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22537         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22538         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22539         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22540         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22541         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22542         final Acceleration bx2 = new Acceleration(0.0,
22543                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22544         calibrator.getInitialBiasXAsAcceleration(bx2);
22545         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22546         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22547         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22548         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22549         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22550         final Acceleration by2 = new Acceleration(0.0,
22551                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22552         calibrator.getInitialBiasYAsAcceleration(by2);
22553         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22554         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22555         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22556         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22557         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22558         final Acceleration bz2 = new Acceleration(0.0,
22559                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22560         calibrator.getInitialBiasZAsAcceleration(bz2);
22561         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22562         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22563         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22564         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22565         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22566         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22567         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22568         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22569         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22570         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22571         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22572         final double[] bias1 = calibrator.getInitialBias();
22573         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22574         final double[] bias2 = new double[3];
22575         calibrator.getInitialBias(bias2);
22576         assertArrayEquals(bias1, bias2, 0.0);
22577         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22578         assertEquals(b1, ba);
22579         final Matrix b2 = new Matrix(3, 1);
22580         calibrator.getInitialBiasAsMatrix(b2);
22581         assertEquals(b1, b2);
22582         final Matrix ma1 = calibrator.getInitialMa();
22583         assertEquals(ma1, new Matrix(3, 3));
22584         final Matrix ma2 = new Matrix(3, 3);
22585         calibrator.getInitialMa(ma2);
22586         assertEquals(ma1, ma2);
22587         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22588         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22589         final NEDPosition nedPosition1 = new NEDPosition();
22590         assertTrue(calibrator.getNedPosition(nedPosition1));
22591         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22592         assertNull(calibrator.getMeasurements());
22593         assertTrue(calibrator.isCommonAxisUsed());
22594         assertNull(calibrator.getListener());
22595         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
22596         assertFalse(calibrator.isReady());
22597         assertFalse(calibrator.isRunning());
22598         assertNull(calibrator.getEstimatedBiases());
22599         assertFalse(calibrator.getEstimatedBiases(null));
22600         assertNull(calibrator.getEstimatedBiasesAsMatrix());
22601         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22602         assertNull(calibrator.getEstimatedBiasFx());
22603         assertNull(calibrator.getEstimatedBiasFy());
22604         assertNull(calibrator.getEstimatedBiasFz());
22605         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22606         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22607         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22608         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22609         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22610         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22611         assertNull(calibrator.getEstimatedMa());
22612         assertNull(calibrator.getEstimatedSx());
22613         assertNull(calibrator.getEstimatedSy());
22614         assertNull(calibrator.getEstimatedSz());
22615         assertNull(calibrator.getEstimatedMxy());
22616         assertNull(calibrator.getEstimatedMxz());
22617         assertNull(calibrator.getEstimatedMyx());
22618         assertNull(calibrator.getEstimatedMyz());
22619         assertNull(calibrator.getEstimatedMzx());
22620         assertNull(calibrator.getEstimatedMzy());
22621         assertNull(calibrator.getEstimatedCovariance());
22622         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22623         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22624                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22625         assertNotNull(calibrator.getGroundTruthGravityNorm());
22626         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22627         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22628         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22629                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22630         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22631         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22632         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22633     }
22634 
22635     @Test
22636     public void testConstructor178() throws WrongSizeException {
22637         final Matrix ba = generateBa();
22638         final double biasX = ba.getElementAtIndex(0);
22639         final double biasY = ba.getElementAtIndex(1);
22640         final double biasZ = ba.getElementAtIndex(2);
22641 
22642         final Acceleration bx = new Acceleration(biasX,
22643                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22644         final Acceleration by = new Acceleration(biasY,
22645                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22646         final Acceleration bz = new Acceleration(biasZ,
22647                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22648 
22649         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22650         final double latitude = Math.toRadians(
22651                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22652         final double longitude = Math.toRadians(
22653                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22654         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22655         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22656         final NEDVelocity nedVelocity = new NEDVelocity();
22657         final ECEFPosition ecefPosition = new ECEFPosition();
22658         final ECEFVelocity ecefVelocity = new ECEFVelocity();
22659         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22660                 ecefPosition, ecefVelocity);
22661 
22662         final KnownPositionAccelerometerCalibrator calibrator =
22663                 new KnownPositionAccelerometerCalibrator(nedPosition,
22664                         true, bx, by, bz, this);
22665 
22666         // check default values
22667         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22668         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22669         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22670         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22671         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22672         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22673         final Acceleration bx2 = new Acceleration(0.0,
22674                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22675         calibrator.getInitialBiasXAsAcceleration(bx2);
22676         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22677         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22678         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22679         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22680         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22681         final Acceleration by2 = new Acceleration(0.0,
22682                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22683         calibrator.getInitialBiasYAsAcceleration(by2);
22684         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22685         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22686         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22687         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22688         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22689         final Acceleration bz2 = new Acceleration(0.0,
22690                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22691         calibrator.getInitialBiasZAsAcceleration(bz2);
22692         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22693         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22694         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22695         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22696         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22697         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22698         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22699         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22700         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22701         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22702         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22703         final double[] bias1 = calibrator.getInitialBias();
22704         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22705         final double[] bias2 = new double[3];
22706         calibrator.getInitialBias(bias2);
22707         assertArrayEquals(bias1, bias2, 0.0);
22708         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22709         assertEquals(b1, ba);
22710         final Matrix b2 = new Matrix(3, 1);
22711         calibrator.getInitialBiasAsMatrix(b2);
22712         assertEquals(b1, b2);
22713         final Matrix ma1 = calibrator.getInitialMa();
22714         assertEquals(ma1, new Matrix(3, 3));
22715         final Matrix ma2 = new Matrix(3, 3);
22716         calibrator.getInitialMa(ma2);
22717         assertEquals(ma1, ma2);
22718         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22719         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22720         final NEDPosition nedPosition1 = new NEDPosition();
22721         assertTrue(calibrator.getNedPosition(nedPosition1));
22722         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22723         assertNull(calibrator.getMeasurements());
22724         assertTrue(calibrator.isCommonAxisUsed());
22725         assertSame(calibrator.getListener(), this);
22726         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
22727         assertFalse(calibrator.isReady());
22728         assertFalse(calibrator.isRunning());
22729         assertNull(calibrator.getEstimatedBiases());
22730         assertFalse(calibrator.getEstimatedBiases(null));
22731         assertNull(calibrator.getEstimatedBiasesAsMatrix());
22732         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22733         assertNull(calibrator.getEstimatedBiasFx());
22734         assertNull(calibrator.getEstimatedBiasFy());
22735         assertNull(calibrator.getEstimatedBiasFz());
22736         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22737         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22738         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22739         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22740         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22741         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22742         assertNull(calibrator.getEstimatedMa());
22743         assertNull(calibrator.getEstimatedSx());
22744         assertNull(calibrator.getEstimatedSy());
22745         assertNull(calibrator.getEstimatedSz());
22746         assertNull(calibrator.getEstimatedMxy());
22747         assertNull(calibrator.getEstimatedMxz());
22748         assertNull(calibrator.getEstimatedMyx());
22749         assertNull(calibrator.getEstimatedMyz());
22750         assertNull(calibrator.getEstimatedMzx());
22751         assertNull(calibrator.getEstimatedMzy());
22752         assertNull(calibrator.getEstimatedCovariance());
22753         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22754         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22755                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22756         assertNotNull(calibrator.getGroundTruthGravityNorm());
22757         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22758         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22759         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22760                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22761         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22762         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22763         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22764     }
22765 
22766     @Test
22767     public void testConstructor179() throws WrongSizeException {
22768         final Collection<StandardDeviationBodyKinematics> measurements =
22769                 Collections.emptyList();
22770 
22771         final Matrix ba = generateBa();
22772         final double biasX = ba.getElementAtIndex(0);
22773         final double biasY = ba.getElementAtIndex(1);
22774         final double biasZ = ba.getElementAtIndex(2);
22775 
22776         final Acceleration bx = new Acceleration(biasX,
22777                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22778         final Acceleration by = new Acceleration(biasY,
22779                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22780         final Acceleration bz = new Acceleration(biasZ,
22781                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22782 
22783         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22784         final double latitude = Math.toRadians(
22785                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22786         final double longitude = Math.toRadians(
22787                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22788         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22789         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22790         final NEDVelocity nedVelocity = new NEDVelocity();
22791         final ECEFPosition ecefPosition = new ECEFPosition();
22792         final ECEFVelocity ecefVelocity = new ECEFVelocity();
22793         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22794                 ecefPosition, ecefVelocity);
22795 
22796         final KnownPositionAccelerometerCalibrator calibrator =
22797                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
22798                         true, bx, by, bz);
22799 
22800         // check default values
22801         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22802         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22803         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22804         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22805         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22806         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22807         final Acceleration bx2 = new Acceleration(0.0,
22808                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22809         calibrator.getInitialBiasXAsAcceleration(bx2);
22810         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22811         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22812         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22813         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22814         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22815         final Acceleration by2 = new Acceleration(0.0,
22816                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22817         calibrator.getInitialBiasYAsAcceleration(by2);
22818         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22819         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22820         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22821         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22822         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22823         final Acceleration bz2 = new Acceleration(0.0,
22824                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22825         calibrator.getInitialBiasZAsAcceleration(bz2);
22826         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22827         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22828         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22829         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22830         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22831         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22832         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22833         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22834         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22835         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22836         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22837         final double[] bias1 = calibrator.getInitialBias();
22838         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22839         final double[] bias2 = new double[3];
22840         calibrator.getInitialBias(bias2);
22841         assertArrayEquals(bias1, bias2, 0.0);
22842         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22843         assertEquals(b1, ba);
22844         final Matrix b2 = new Matrix(3, 1);
22845         calibrator.getInitialBiasAsMatrix(b2);
22846         assertEquals(b1, b2);
22847         final Matrix ma1 = calibrator.getInitialMa();
22848         assertEquals(ma1, new Matrix(3, 3));
22849         final Matrix ma2 = new Matrix(3, 3);
22850         calibrator.getInitialMa(ma2);
22851         assertEquals(ma1, ma2);
22852         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22853         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22854         final NEDPosition nedPosition1 = new NEDPosition();
22855         assertTrue(calibrator.getNedPosition(nedPosition1));
22856         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22857         assertSame(calibrator.getMeasurements(), measurements);
22858         assertTrue(calibrator.isCommonAxisUsed());
22859         assertNull(calibrator.getListener());
22860         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
22861         assertFalse(calibrator.isReady());
22862         assertFalse(calibrator.isRunning());
22863         assertNull(calibrator.getEstimatedBiases());
22864         assertFalse(calibrator.getEstimatedBiases(null));
22865         assertNull(calibrator.getEstimatedBiasesAsMatrix());
22866         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22867         assertNull(calibrator.getEstimatedBiasFx());
22868         assertNull(calibrator.getEstimatedBiasFy());
22869         assertNull(calibrator.getEstimatedBiasFz());
22870         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22871         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22872         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22873         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22874         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22875         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22876         assertNull(calibrator.getEstimatedMa());
22877         assertNull(calibrator.getEstimatedSx());
22878         assertNull(calibrator.getEstimatedSy());
22879         assertNull(calibrator.getEstimatedSz());
22880         assertNull(calibrator.getEstimatedMxy());
22881         assertNull(calibrator.getEstimatedMxz());
22882         assertNull(calibrator.getEstimatedMyx());
22883         assertNull(calibrator.getEstimatedMyz());
22884         assertNull(calibrator.getEstimatedMzx());
22885         assertNull(calibrator.getEstimatedMzy());
22886         assertNull(calibrator.getEstimatedCovariance());
22887         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22888         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22889                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22890         assertNotNull(calibrator.getGroundTruthGravityNorm());
22891         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22892         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22893         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22894                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22895         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22896         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22897         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22898     }
22899 
22900     @Test
22901     public void testConstructor180() throws WrongSizeException {
22902         final Collection<StandardDeviationBodyKinematics> measurements =
22903                 Collections.emptyList();
22904 
22905         final Matrix ba = generateBa();
22906         final double biasX = ba.getElementAtIndex(0);
22907         final double biasY = ba.getElementAtIndex(1);
22908         final double biasZ = ba.getElementAtIndex(2);
22909 
22910         final Acceleration bx = new Acceleration(biasX,
22911                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22912         final Acceleration by = new Acceleration(biasY,
22913                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22914         final Acceleration bz = new Acceleration(biasZ,
22915                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22916 
22917         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22918         final double latitude = Math.toRadians(
22919                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22920         final double longitude = Math.toRadians(
22921                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22922         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22923         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22924         final NEDVelocity nedVelocity = new NEDVelocity();
22925         final ECEFPosition ecefPosition = new ECEFPosition();
22926         final ECEFVelocity ecefVelocity = new ECEFVelocity();
22927         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22928                 ecefPosition, ecefVelocity);
22929 
22930         final KnownPositionAccelerometerCalibrator calibrator =
22931                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
22932                         true, bx, by, bz, this);
22933 
22934         // check default values
22935         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22936         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22937         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22938         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22939         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22940         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22941         final Acceleration bx2 = new Acceleration(0.0,
22942                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22943         calibrator.getInitialBiasXAsAcceleration(bx2);
22944         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22945         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22946         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22947         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22948         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22949         final Acceleration by2 = new Acceleration(0.0,
22950                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22951         calibrator.getInitialBiasYAsAcceleration(by2);
22952         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22953         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22954         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22955         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22956         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22957         final Acceleration bz2 = new Acceleration(0.0,
22958                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22959         calibrator.getInitialBiasZAsAcceleration(bz2);
22960         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22961         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22962         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22963         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22964         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22965         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22966         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22967         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22968         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22969         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22970         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22971         final double[] bias1 = calibrator.getInitialBias();
22972         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22973         final double[] bias2 = new double[3];
22974         calibrator.getInitialBias(bias2);
22975         assertArrayEquals(bias1, bias2, 0.0);
22976         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22977         assertEquals(b1, ba);
22978         final Matrix b2 = new Matrix(3, 1);
22979         calibrator.getInitialBiasAsMatrix(b2);
22980         assertEquals(b1, b2);
22981         final Matrix ma1 = calibrator.getInitialMa();
22982         assertEquals(ma1, new Matrix(3, 3));
22983         final Matrix ma2 = new Matrix(3, 3);
22984         calibrator.getInitialMa(ma2);
22985         assertEquals(ma1, ma2);
22986         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22987         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22988         final NEDPosition nedPosition1 = new NEDPosition();
22989         assertTrue(calibrator.getNedPosition(nedPosition1));
22990         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22991         assertSame(calibrator.getMeasurements(), measurements);
22992         assertTrue(calibrator.isCommonAxisUsed());
22993         assertSame(calibrator.getListener(), this);
22994         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
22995         assertFalse(calibrator.isReady());
22996         assertFalse(calibrator.isRunning());
22997         assertNull(calibrator.getEstimatedBiases());
22998         assertFalse(calibrator.getEstimatedBiases(null));
22999         assertNull(calibrator.getEstimatedBiasesAsMatrix());
23000         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23001         assertNull(calibrator.getEstimatedBiasFx());
23002         assertNull(calibrator.getEstimatedBiasFy());
23003         assertNull(calibrator.getEstimatedBiasFz());
23004         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23005         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23006         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23007         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23008         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23009         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23010         assertNull(calibrator.getEstimatedMa());
23011         assertNull(calibrator.getEstimatedSx());
23012         assertNull(calibrator.getEstimatedSy());
23013         assertNull(calibrator.getEstimatedSz());
23014         assertNull(calibrator.getEstimatedMxy());
23015         assertNull(calibrator.getEstimatedMxz());
23016         assertNull(calibrator.getEstimatedMyx());
23017         assertNull(calibrator.getEstimatedMyz());
23018         assertNull(calibrator.getEstimatedMzx());
23019         assertNull(calibrator.getEstimatedMzy());
23020         assertNull(calibrator.getEstimatedCovariance());
23021         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23022         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23023                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23024         assertNotNull(calibrator.getGroundTruthGravityNorm());
23025         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23026         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23027         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23028                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23029         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23030         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23031         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23032     }
23033 
23034     @Test
23035     public void testConstructor181() throws WrongSizeException {
23036         final Matrix ba = generateBa();
23037         final double biasX = ba.getElementAtIndex(0);
23038         final double biasY = ba.getElementAtIndex(1);
23039         final double biasZ = ba.getElementAtIndex(2);
23040 
23041         final Matrix ma = generateMaCommonAxis();
23042         final double sx = ma.getElementAt(0, 0);
23043         final double sy = ma.getElementAt(1, 1);
23044         final double sz = ma.getElementAt(2, 2);
23045 
23046         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23047         final double latitude = Math.toRadians(
23048                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23049         final double longitude = Math.toRadians(
23050                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23051         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23052         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23053         final NEDVelocity nedVelocity = new NEDVelocity();
23054         final ECEFPosition ecefPosition = new ECEFPosition();
23055         final ECEFVelocity ecefVelocity = new ECEFVelocity();
23056         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23057                 ecefPosition, ecefVelocity);
23058 
23059         final KnownPositionAccelerometerCalibrator calibrator =
23060                 new KnownPositionAccelerometerCalibrator(nedPosition,
23061                         biasX, biasY, biasZ, sx, sy, sz);
23062 
23063         // check default values
23064         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23065         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23066         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23067         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23068         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23069         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23070         final Acceleration bx2 = new Acceleration(0.0,
23071                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23072         calibrator.getInitialBiasXAsAcceleration(bx2);
23073         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23074         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23075         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23076         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23077         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23078         final Acceleration by2 = new Acceleration(0.0,
23079                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23080         calibrator.getInitialBiasYAsAcceleration(by2);
23081         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23082         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23083         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23084         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23085         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23086         final Acceleration bz2 = new Acceleration(0.0,
23087                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23088         calibrator.getInitialBiasZAsAcceleration(bz2);
23089         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23090         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23091         assertEquals(calibrator.getInitialSx(), sx, 0.0);
23092         assertEquals(calibrator.getInitialSy(), sy, 0.0);
23093         assertEquals(calibrator.getInitialSz(), sz, 0.0);
23094         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23095         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23096         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23097         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23098         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23099         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23100         final double[] bias1 = calibrator.getInitialBias();
23101         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23102         final double[] bias2 = new double[3];
23103         calibrator.getInitialBias(bias2);
23104         assertArrayEquals(bias1, bias2, 0.0);
23105         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23106         assertEquals(b1, ba);
23107         final Matrix b2 = new Matrix(3, 1);
23108         calibrator.getInitialBiasAsMatrix(b2);
23109         assertEquals(b1, b2);
23110         final Matrix ma1 = calibrator.getInitialMa();
23111         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
23112         final Matrix ma2 = new Matrix(3, 3);
23113         calibrator.getInitialMa(ma2);
23114         assertEquals(ma1, ma2);
23115         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23116         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23117         final NEDPosition nedPosition1 = new NEDPosition();
23118         assertTrue(calibrator.getNedPosition(nedPosition1));
23119         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23120         assertNull(calibrator.getMeasurements());
23121         assertFalse(calibrator.isCommonAxisUsed());
23122         assertNull(calibrator.getListener());
23123         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
23124         assertFalse(calibrator.isReady());
23125         assertFalse(calibrator.isRunning());
23126         assertNull(calibrator.getEstimatedBiases());
23127         assertFalse(calibrator.getEstimatedBiases(null));
23128         assertNull(calibrator.getEstimatedBiasesAsMatrix());
23129         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23130         assertNull(calibrator.getEstimatedBiasFx());
23131         assertNull(calibrator.getEstimatedBiasFy());
23132         assertNull(calibrator.getEstimatedBiasFz());
23133         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23134         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23135         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23136         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23137         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23138         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23139         assertNull(calibrator.getEstimatedMa());
23140         assertNull(calibrator.getEstimatedSx());
23141         assertNull(calibrator.getEstimatedSy());
23142         assertNull(calibrator.getEstimatedSz());
23143         assertNull(calibrator.getEstimatedMxy());
23144         assertNull(calibrator.getEstimatedMxz());
23145         assertNull(calibrator.getEstimatedMyx());
23146         assertNull(calibrator.getEstimatedMyz());
23147         assertNull(calibrator.getEstimatedMzx());
23148         assertNull(calibrator.getEstimatedMzy());
23149         assertNull(calibrator.getEstimatedCovariance());
23150         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23151         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23152                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23153         assertNotNull(calibrator.getGroundTruthGravityNorm());
23154         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23155         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23156         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23157                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23158         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23159         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23160         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23161     }
23162 
23163     @Test
23164     public void testConstructor182() throws WrongSizeException {
23165         final Collection<StandardDeviationBodyKinematics> measurements =
23166                 Collections.emptyList();
23167 
23168         final Matrix ba = generateBa();
23169         final double biasX = ba.getElementAtIndex(0);
23170         final double biasY = ba.getElementAtIndex(1);
23171         final double biasZ = ba.getElementAtIndex(2);
23172 
23173         final Matrix ma = generateMaCommonAxis();
23174         final double sx = ma.getElementAt(0, 0);
23175         final double sy = ma.getElementAt(1, 1);
23176         final double sz = ma.getElementAt(2, 2);
23177 
23178         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23179         final double latitude = Math.toRadians(
23180                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23181         final double longitude = Math.toRadians(
23182                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23183         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23184         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23185         final NEDVelocity nedVelocity = new NEDVelocity();
23186         final ECEFPosition ecefPosition = new ECEFPosition();
23187         final ECEFVelocity ecefVelocity = new ECEFVelocity();
23188         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23189                 ecefPosition, ecefVelocity);
23190 
23191         final KnownPositionAccelerometerCalibrator calibrator =
23192                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
23193                         biasX, biasY, biasZ, sx, sy, sz);
23194 
23195         // check default values
23196         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23197         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23198         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23199         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23200         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23201         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23202         final Acceleration bx2 = new Acceleration(0.0,
23203                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23204         calibrator.getInitialBiasXAsAcceleration(bx2);
23205         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23206         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23207         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23208         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23209         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23210         final Acceleration by2 = new Acceleration(0.0,
23211                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23212         calibrator.getInitialBiasYAsAcceleration(by2);
23213         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23214         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23215         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23216         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23217         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23218         final Acceleration bz2 = new Acceleration(0.0,
23219                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23220         calibrator.getInitialBiasZAsAcceleration(bz2);
23221         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23222         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23223         assertEquals(calibrator.getInitialSx(), sx, 0.0);
23224         assertEquals(calibrator.getInitialSy(), sy, 0.0);
23225         assertEquals(calibrator.getInitialSz(), sz, 0.0);
23226         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23227         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23228         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23229         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23230         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23231         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23232         final double[] bias1 = calibrator.getInitialBias();
23233         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23234         final double[] bias2 = new double[3];
23235         calibrator.getInitialBias(bias2);
23236         assertArrayEquals(bias1, bias2, 0.0);
23237         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23238         assertEquals(b1, ba);
23239         final Matrix b2 = new Matrix(3, 1);
23240         calibrator.getInitialBiasAsMatrix(b2);
23241         assertEquals(b1, b2);
23242         final Matrix ma1 = calibrator.getInitialMa();
23243         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
23244         final Matrix ma2 = new Matrix(3, 3);
23245         calibrator.getInitialMa(ma2);
23246         assertEquals(ma1, ma2);
23247         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23248         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23249         final NEDPosition nedPosition1 = new NEDPosition();
23250         assertTrue(calibrator.getNedPosition(nedPosition1));
23251         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23252         assertSame(calibrator.getMeasurements(), measurements);
23253         assertFalse(calibrator.isCommonAxisUsed());
23254         assertNull(calibrator.getListener());
23255         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
23256         assertFalse(calibrator.isReady());
23257         assertFalse(calibrator.isRunning());
23258         assertNull(calibrator.getEstimatedBiases());
23259         assertFalse(calibrator.getEstimatedBiases(null));
23260         assertNull(calibrator.getEstimatedBiasesAsMatrix());
23261         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23262         assertNull(calibrator.getEstimatedBiasFx());
23263         assertNull(calibrator.getEstimatedBiasFy());
23264         assertNull(calibrator.getEstimatedBiasFz());
23265         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23266         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23267         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23268         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23269         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23270         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23271         assertNull(calibrator.getEstimatedMa());
23272         assertNull(calibrator.getEstimatedSx());
23273         assertNull(calibrator.getEstimatedSy());
23274         assertNull(calibrator.getEstimatedSz());
23275         assertNull(calibrator.getEstimatedMxy());
23276         assertNull(calibrator.getEstimatedMxz());
23277         assertNull(calibrator.getEstimatedMyx());
23278         assertNull(calibrator.getEstimatedMyz());
23279         assertNull(calibrator.getEstimatedMzx());
23280         assertNull(calibrator.getEstimatedMzy());
23281         assertNull(calibrator.getEstimatedCovariance());
23282         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23283         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23284                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23285         assertNotNull(calibrator.getGroundTruthGravityNorm());
23286         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23287         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23288         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23289                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23290         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23291         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23292         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23293     }
23294 
23295     @Test
23296     public void testConstructor183() throws WrongSizeException {
23297         final Collection<StandardDeviationBodyKinematics> measurements =
23298                 Collections.emptyList();
23299 
23300         final Matrix ba = generateBa();
23301         final double biasX = ba.getElementAtIndex(0);
23302         final double biasY = ba.getElementAtIndex(1);
23303         final double biasZ = ba.getElementAtIndex(2);
23304 
23305         final Matrix ma = generateMaCommonAxis();
23306         final double sx = ma.getElementAt(0, 0);
23307         final double sy = ma.getElementAt(1, 1);
23308         final double sz = ma.getElementAt(2, 2);
23309 
23310         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23311         final double latitude = Math.toRadians(
23312                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23313         final double longitude = Math.toRadians(
23314                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23315         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23316         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23317         final NEDVelocity nedVelocity = new NEDVelocity();
23318         final ECEFPosition ecefPosition = new ECEFPosition();
23319         final ECEFVelocity ecefVelocity = new ECEFVelocity();
23320         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23321                 ecefPosition, ecefVelocity);
23322 
23323         final KnownPositionAccelerometerCalibrator calibrator =
23324                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
23325                         biasX, biasY, biasZ, sx, sy, sz, this);
23326 
23327         // check default values
23328         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23329         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23330         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23331         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23332         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23333         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23334         final Acceleration bx2 = new Acceleration(0.0,
23335                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23336         calibrator.getInitialBiasXAsAcceleration(bx2);
23337         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23338         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23339         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23340         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23341         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23342         final Acceleration by2 = new Acceleration(0.0,
23343                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23344         calibrator.getInitialBiasYAsAcceleration(by2);
23345         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23346         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23347         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23348         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23349         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23350         final Acceleration bz2 = new Acceleration(0.0,
23351                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23352         calibrator.getInitialBiasZAsAcceleration(bz2);
23353         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23354         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23355         assertEquals(calibrator.getInitialSx(), sx, 0.0);
23356         assertEquals(calibrator.getInitialSy(), sy, 0.0);
23357         assertEquals(calibrator.getInitialSz(), sz, 0.0);
23358         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23359         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23360         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23361         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23362         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23363         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23364         final double[] bias1 = calibrator.getInitialBias();
23365         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23366         final double[] bias2 = new double[3];
23367         calibrator.getInitialBias(bias2);
23368         assertArrayEquals(bias1, bias2, 0.0);
23369         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23370         assertEquals(b1, ba);
23371         final Matrix b2 = new Matrix(3, 1);
23372         calibrator.getInitialBiasAsMatrix(b2);
23373         assertEquals(b1, b2);
23374         final Matrix ma1 = calibrator.getInitialMa();
23375         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
23376         final Matrix ma2 = new Matrix(3, 3);
23377         calibrator.getInitialMa(ma2);
23378         assertEquals(ma1, ma2);
23379         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23380         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23381         final NEDPosition nedPosition1 = new NEDPosition();
23382         assertTrue(calibrator.getNedPosition(nedPosition1));
23383         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23384         assertSame(calibrator.getMeasurements(), measurements);
23385         assertFalse(calibrator.isCommonAxisUsed());
23386         assertSame(calibrator.getListener(), this);
23387         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
23388         assertFalse(calibrator.isReady());
23389         assertFalse(calibrator.isRunning());
23390         assertNull(calibrator.getEstimatedBiases());
23391         assertFalse(calibrator.getEstimatedBiases(null));
23392         assertNull(calibrator.getEstimatedBiasesAsMatrix());
23393         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23394         assertNull(calibrator.getEstimatedBiasFx());
23395         assertNull(calibrator.getEstimatedBiasFy());
23396         assertNull(calibrator.getEstimatedBiasFz());
23397         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23398         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23399         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23400         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23401         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23402         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23403         assertNull(calibrator.getEstimatedMa());
23404         assertNull(calibrator.getEstimatedSx());
23405         assertNull(calibrator.getEstimatedSy());
23406         assertNull(calibrator.getEstimatedSz());
23407         assertNull(calibrator.getEstimatedMxy());
23408         assertNull(calibrator.getEstimatedMxz());
23409         assertNull(calibrator.getEstimatedMyx());
23410         assertNull(calibrator.getEstimatedMyz());
23411         assertNull(calibrator.getEstimatedMzx());
23412         assertNull(calibrator.getEstimatedMzy());
23413         assertNull(calibrator.getEstimatedCovariance());
23414         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23415         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23416                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23417         assertNotNull(calibrator.getGroundTruthGravityNorm());
23418         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23419         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23420         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23421                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23422         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23423         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23424         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23425     }
23426 
23427     @Test
23428     public void testConstructor184() throws WrongSizeException {
23429         final Matrix ba = generateBa();
23430         final double biasX = ba.getElementAtIndex(0);
23431         final double biasY = ba.getElementAtIndex(1);
23432         final double biasZ = ba.getElementAtIndex(2);
23433 
23434         final Matrix ma = generateMaCommonAxis();
23435         final double sx = ma.getElementAt(0, 0);
23436         final double sy = ma.getElementAt(1, 1);
23437         final double sz = ma.getElementAt(2, 2);
23438 
23439         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23440         final double latitude = Math.toRadians(
23441                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23442         final double longitude = Math.toRadians(
23443                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23444         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23445         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23446         final NEDVelocity nedVelocity = new NEDVelocity();
23447         final ECEFPosition ecefPosition = new ECEFPosition();
23448         final ECEFVelocity ecefVelocity = new ECEFVelocity();
23449         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23450                 ecefPosition, ecefVelocity);
23451 
23452         final KnownPositionAccelerometerCalibrator calibrator =
23453                 new KnownPositionAccelerometerCalibrator(nedPosition,
23454                         true, biasX, biasY, biasZ, sx, sy, sz);
23455 
23456         // check default values
23457         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23458         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23459         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23460         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23461         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23462         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23463         final Acceleration bx2 = new Acceleration(0.0,
23464                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23465         calibrator.getInitialBiasXAsAcceleration(bx2);
23466         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23467         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23468         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23469         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23470         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23471         final Acceleration by2 = new Acceleration(0.0,
23472                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23473         calibrator.getInitialBiasYAsAcceleration(by2);
23474         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23475         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23476         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23477         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23478         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23479         final Acceleration bz2 = new Acceleration(0.0,
23480                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23481         calibrator.getInitialBiasZAsAcceleration(bz2);
23482         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23483         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23484         assertEquals(calibrator.getInitialSx(), sx, 0.0);
23485         assertEquals(calibrator.getInitialSy(), sy, 0.0);
23486         assertEquals(calibrator.getInitialSz(), sz, 0.0);
23487         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23488         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23489         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23490         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23491         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23492         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23493         final double[] bias1 = calibrator.getInitialBias();
23494         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23495         final double[] bias2 = new double[3];
23496         calibrator.getInitialBias(bias2);
23497         assertArrayEquals(bias1, bias2, 0.0);
23498         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23499         assertEquals(b1, ba);
23500         final Matrix b2 = new Matrix(3, 1);
23501         calibrator.getInitialBiasAsMatrix(b2);
23502         assertEquals(b1, b2);
23503         final Matrix ma1 = calibrator.getInitialMa();
23504         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
23505         final Matrix ma2 = new Matrix(3, 3);
23506         calibrator.getInitialMa(ma2);
23507         assertEquals(ma1, ma2);
23508         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23509         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23510         final NEDPosition nedPosition1 = new NEDPosition();
23511         assertTrue(calibrator.getNedPosition(nedPosition1));
23512         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23513         assertNull(calibrator.getMeasurements());
23514         assertTrue(calibrator.isCommonAxisUsed());
23515         assertNull(calibrator.getListener());
23516         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
23517         assertFalse(calibrator.isReady());
23518         assertFalse(calibrator.isRunning());
23519         assertNull(calibrator.getEstimatedBiases());
23520         assertFalse(calibrator.getEstimatedBiases(null));
23521         assertNull(calibrator.getEstimatedBiasesAsMatrix());
23522         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23523         assertNull(calibrator.getEstimatedBiasFx());
23524         assertNull(calibrator.getEstimatedBiasFy());
23525         assertNull(calibrator.getEstimatedBiasFz());
23526         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23527         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23528         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23529         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23530         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23531         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23532         assertNull(calibrator.getEstimatedMa());
23533         assertNull(calibrator.getEstimatedSx());
23534         assertNull(calibrator.getEstimatedSy());
23535         assertNull(calibrator.getEstimatedSz());
23536         assertNull(calibrator.getEstimatedMxy());
23537         assertNull(calibrator.getEstimatedMxz());
23538         assertNull(calibrator.getEstimatedMyx());
23539         assertNull(calibrator.getEstimatedMyz());
23540         assertNull(calibrator.getEstimatedMzx());
23541         assertNull(calibrator.getEstimatedMzy());
23542         assertNull(calibrator.getEstimatedCovariance());
23543         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23544         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23545                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23546         assertNotNull(calibrator.getGroundTruthGravityNorm());
23547         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23548         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23549         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23550                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23551         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23552         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23553         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23554     }
23555 
23556     @Test
23557     public void testConstructor185() throws WrongSizeException {
23558         final Matrix ba = generateBa();
23559         final double biasX = ba.getElementAtIndex(0);
23560         final double biasY = ba.getElementAtIndex(1);
23561         final double biasZ = ba.getElementAtIndex(2);
23562 
23563         final Matrix ma = generateMaCommonAxis();
23564         final double sx = ma.getElementAt(0, 0);
23565         final double sy = ma.getElementAt(1, 1);
23566         final double sz = ma.getElementAt(2, 2);
23567 
23568         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23569         final double latitude = Math.toRadians(
23570                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23571         final double longitude = Math.toRadians(
23572                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23573         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23574         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23575         final NEDVelocity nedVelocity = new NEDVelocity();
23576         final ECEFPosition ecefPosition = new ECEFPosition();
23577         final ECEFVelocity ecefVelocity = new ECEFVelocity();
23578         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23579                 ecefPosition, ecefVelocity);
23580 
23581         final KnownPositionAccelerometerCalibrator calibrator =
23582                 new KnownPositionAccelerometerCalibrator(nedPosition,
23583                         true, biasX, biasY, biasZ, sx, sy, sz,
23584                         this);
23585 
23586         // check default values
23587         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23588         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23589         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23590         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23591         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23592         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23593         final Acceleration bx2 = new Acceleration(0.0,
23594                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23595         calibrator.getInitialBiasXAsAcceleration(bx2);
23596         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23597         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23598         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23599         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23600         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23601         final Acceleration by2 = new Acceleration(0.0,
23602                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23603         calibrator.getInitialBiasYAsAcceleration(by2);
23604         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23605         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23606         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23607         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23608         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23609         final Acceleration bz2 = new Acceleration(0.0,
23610                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23611         calibrator.getInitialBiasZAsAcceleration(bz2);
23612         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23613         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23614         assertEquals(calibrator.getInitialSx(), sx, 0.0);
23615         assertEquals(calibrator.getInitialSy(), sy, 0.0);
23616         assertEquals(calibrator.getInitialSz(), sz, 0.0);
23617         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23618         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23619         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23620         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23621         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23622         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23623         final double[] bias1 = calibrator.getInitialBias();
23624         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23625         final double[] bias2 = new double[3];
23626         calibrator.getInitialBias(bias2);
23627         assertArrayEquals(bias1, bias2, 0.0);
23628         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23629         assertEquals(b1, ba);
23630         final Matrix b2 = new Matrix(3, 1);
23631         calibrator.getInitialBiasAsMatrix(b2);
23632         assertEquals(b1, b2);
23633         final Matrix ma1 = calibrator.getInitialMa();
23634         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
23635         final Matrix ma2 = new Matrix(3, 3);
23636         calibrator.getInitialMa(ma2);
23637         assertEquals(ma1, ma2);
23638         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23639         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23640         final NEDPosition nedPosition1 = new NEDPosition();
23641         assertTrue(calibrator.getNedPosition(nedPosition1));
23642         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23643         assertNull(calibrator.getMeasurements());
23644         assertTrue(calibrator.isCommonAxisUsed());
23645         assertSame(calibrator.getListener(), this);
23646         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
23647         assertFalse(calibrator.isReady());
23648         assertFalse(calibrator.isRunning());
23649         assertNull(calibrator.getEstimatedBiases());
23650         assertFalse(calibrator.getEstimatedBiases(null));
23651         assertNull(calibrator.getEstimatedBiasesAsMatrix());
23652         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23653         assertNull(calibrator.getEstimatedBiasFx());
23654         assertNull(calibrator.getEstimatedBiasFy());
23655         assertNull(calibrator.getEstimatedBiasFz());
23656         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23657         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23658         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23659         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23660         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23661         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23662         assertNull(calibrator.getEstimatedMa());
23663         assertNull(calibrator.getEstimatedSx());
23664         assertNull(calibrator.getEstimatedSy());
23665         assertNull(calibrator.getEstimatedSz());
23666         assertNull(calibrator.getEstimatedMxy());
23667         assertNull(calibrator.getEstimatedMxz());
23668         assertNull(calibrator.getEstimatedMyx());
23669         assertNull(calibrator.getEstimatedMyz());
23670         assertNull(calibrator.getEstimatedMzx());
23671         assertNull(calibrator.getEstimatedMzy());
23672         assertNull(calibrator.getEstimatedCovariance());
23673         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23674         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23675                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23676         assertNotNull(calibrator.getGroundTruthGravityNorm());
23677         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23678         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23679         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23680                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23681         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23682         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23683         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23684     }
23685 
23686     @Test
23687     public void testConstructor186() throws WrongSizeException {
23688         final Collection<StandardDeviationBodyKinematics> measurements =
23689                 Collections.emptyList();
23690 
23691         final Matrix ba = generateBa();
23692         final double biasX = ba.getElementAtIndex(0);
23693         final double biasY = ba.getElementAtIndex(1);
23694         final double biasZ = ba.getElementAtIndex(2);
23695 
23696         final Matrix ma = generateMaCommonAxis();
23697         final double sx = ma.getElementAt(0, 0);
23698         final double sy = ma.getElementAt(1, 1);
23699         final double sz = ma.getElementAt(2, 2);
23700 
23701         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23702         final double latitude = Math.toRadians(
23703                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23704         final double longitude = Math.toRadians(
23705                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23706         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23707         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23708         final NEDVelocity nedVelocity = new NEDVelocity();
23709         final ECEFPosition ecefPosition = new ECEFPosition();
23710         final ECEFVelocity ecefVelocity = new ECEFVelocity();
23711         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23712                 ecefPosition, ecefVelocity);
23713 
23714         final KnownPositionAccelerometerCalibrator calibrator =
23715                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
23716                         true, biasX, biasY, biasZ, sx, sy, sz);
23717 
23718         // check default values
23719         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23720         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23721         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23722         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23723         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23724         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23725         final Acceleration bx2 = new Acceleration(0.0,
23726                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23727         calibrator.getInitialBiasXAsAcceleration(bx2);
23728         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23729         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23730         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23731         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23732         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23733         final Acceleration by2 = new Acceleration(0.0,
23734                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23735         calibrator.getInitialBiasYAsAcceleration(by2);
23736         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23737         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23738         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23739         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23740         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23741         final Acceleration bz2 = new Acceleration(0.0,
23742                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23743         calibrator.getInitialBiasZAsAcceleration(bz2);
23744         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23745         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23746         assertEquals(calibrator.getInitialSx(), sx, 0.0);
23747         assertEquals(calibrator.getInitialSy(), sy, 0.0);
23748         assertEquals(calibrator.getInitialSz(), sz, 0.0);
23749         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23750         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23751         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23752         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23753         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23754         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23755         final double[] bias1 = calibrator.getInitialBias();
23756         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23757         final double[] bias2 = new double[3];
23758         calibrator.getInitialBias(bias2);
23759         assertArrayEquals(bias1, bias2, 0.0);
23760         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23761         assertEquals(b1, ba);
23762         final Matrix b2 = new Matrix(3, 1);
23763         calibrator.getInitialBiasAsMatrix(b2);
23764         assertEquals(b1, b2);
23765         final Matrix ma1 = calibrator.getInitialMa();
23766         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
23767         final Matrix ma2 = new Matrix(3, 3);
23768         calibrator.getInitialMa(ma2);
23769         assertEquals(ma1, ma2);
23770         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23771         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23772         final NEDPosition nedPosition1 = new NEDPosition();
23773         assertTrue(calibrator.getNedPosition(nedPosition1));
23774         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23775         assertSame(calibrator.getMeasurements(), measurements);
23776         assertTrue(calibrator.isCommonAxisUsed());
23777         assertNull(calibrator.getListener());
23778         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
23779         assertFalse(calibrator.isReady());
23780         assertFalse(calibrator.isRunning());
23781         assertNull(calibrator.getEstimatedBiases());
23782         assertFalse(calibrator.getEstimatedBiases(null));
23783         assertNull(calibrator.getEstimatedBiasesAsMatrix());
23784         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23785         assertNull(calibrator.getEstimatedBiasFx());
23786         assertNull(calibrator.getEstimatedBiasFy());
23787         assertNull(calibrator.getEstimatedBiasFz());
23788         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23789         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23790         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23791         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23792         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23793         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23794         assertNull(calibrator.getEstimatedMa());
23795         assertNull(calibrator.getEstimatedSx());
23796         assertNull(calibrator.getEstimatedSy());
23797         assertNull(calibrator.getEstimatedSz());
23798         assertNull(calibrator.getEstimatedMxy());
23799         assertNull(calibrator.getEstimatedMxz());
23800         assertNull(calibrator.getEstimatedMyx());
23801         assertNull(calibrator.getEstimatedMyz());
23802         assertNull(calibrator.getEstimatedMzx());
23803         assertNull(calibrator.getEstimatedMzy());
23804         assertNull(calibrator.getEstimatedCovariance());
23805         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23806         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23807                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23808         assertNotNull(calibrator.getGroundTruthGravityNorm());
23809         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23810         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23811         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23812                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23813         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23814         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23815         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23816     }
23817 
23818     @Test
23819     public void testConstructor187() throws WrongSizeException {
23820         final Collection<StandardDeviationBodyKinematics> measurements =
23821                 Collections.emptyList();
23822 
23823         final Matrix ba = generateBa();
23824         final double biasX = ba.getElementAtIndex(0);
23825         final double biasY = ba.getElementAtIndex(1);
23826         final double biasZ = ba.getElementAtIndex(2);
23827 
23828         final Matrix ma = generateMaCommonAxis();
23829         final double sx = ma.getElementAt(0, 0);
23830         final double sy = ma.getElementAt(1, 1);
23831         final double sz = ma.getElementAt(2, 2);
23832 
23833         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23834         final double latitude = Math.toRadians(
23835                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23836         final double longitude = Math.toRadians(
23837                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23838         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23839         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23840         final NEDVelocity nedVelocity = new NEDVelocity();
23841         final ECEFPosition ecefPosition = new ECEFPosition();
23842         final ECEFVelocity ecefVelocity = new ECEFVelocity();
23843         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23844                 ecefPosition, ecefVelocity);
23845 
23846         final KnownPositionAccelerometerCalibrator calibrator =
23847                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
23848                         true, biasX, biasY, biasZ, sx, sy, sz,
23849                         this);
23850 
23851         // check default values
23852         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23853         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23854         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23855         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23856         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23857         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23858         final Acceleration bx2 = new Acceleration(0.0,
23859                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23860         calibrator.getInitialBiasXAsAcceleration(bx2);
23861         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23862         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23863         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23864         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23865         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23866         final Acceleration by2 = new Acceleration(0.0,
23867                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23868         calibrator.getInitialBiasYAsAcceleration(by2);
23869         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23870         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23871         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23872         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23873         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23874         final Acceleration bz2 = new Acceleration(0.0,
23875                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23876         calibrator.getInitialBiasZAsAcceleration(bz2);
23877         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23878         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23879         assertEquals(calibrator.getInitialSx(), sx, 0.0);
23880         assertEquals(calibrator.getInitialSy(), sy, 0.0);
23881         assertEquals(calibrator.getInitialSz(), sz, 0.0);
23882         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23883         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23884         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23885         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23886         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23887         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23888         final double[] bias1 = calibrator.getInitialBias();
23889         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23890         final double[] bias2 = new double[3];
23891         calibrator.getInitialBias(bias2);
23892         assertArrayEquals(bias1, bias2, 0.0);
23893         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23894         assertEquals(b1, ba);
23895         final Matrix b2 = new Matrix(3, 1);
23896         calibrator.getInitialBiasAsMatrix(b2);
23897         assertEquals(b1, b2);
23898         final Matrix ma1 = calibrator.getInitialMa();
23899         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
23900         final Matrix ma2 = new Matrix(3, 3);
23901         calibrator.getInitialMa(ma2);
23902         assertEquals(ma1, ma2);
23903         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23904         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23905         final NEDPosition nedPosition1 = new NEDPosition();
23906         assertTrue(calibrator.getNedPosition(nedPosition1));
23907         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23908         assertSame(calibrator.getMeasurements(), measurements);
23909         assertTrue(calibrator.isCommonAxisUsed());
23910         assertSame(calibrator.getListener(), this);
23911         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
23912         assertFalse(calibrator.isReady());
23913         assertFalse(calibrator.isRunning());
23914         assertNull(calibrator.getEstimatedBiases());
23915         assertFalse(calibrator.getEstimatedBiases(null));
23916         assertNull(calibrator.getEstimatedBiasesAsMatrix());
23917         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23918         assertNull(calibrator.getEstimatedBiasFx());
23919         assertNull(calibrator.getEstimatedBiasFy());
23920         assertNull(calibrator.getEstimatedBiasFz());
23921         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23922         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23923         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23924         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23925         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23926         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23927         assertNull(calibrator.getEstimatedMa());
23928         assertNull(calibrator.getEstimatedSx());
23929         assertNull(calibrator.getEstimatedSy());
23930         assertNull(calibrator.getEstimatedSz());
23931         assertNull(calibrator.getEstimatedMxy());
23932         assertNull(calibrator.getEstimatedMxz());
23933         assertNull(calibrator.getEstimatedMyx());
23934         assertNull(calibrator.getEstimatedMyz());
23935         assertNull(calibrator.getEstimatedMzx());
23936         assertNull(calibrator.getEstimatedMzy());
23937         assertNull(calibrator.getEstimatedCovariance());
23938         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23939         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23940                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23941         assertNotNull(calibrator.getGroundTruthGravityNorm());
23942         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23943         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23944         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23945                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23946         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23947         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23948         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23949     }
23950 
23951     @Test
23952     public void testConstructor188() throws WrongSizeException {
23953         final Matrix ba = generateBa();
23954         final double biasX = ba.getElementAtIndex(0);
23955         final double biasY = ba.getElementAtIndex(1);
23956         final double biasZ = ba.getElementAtIndex(2);
23957 
23958         final Acceleration bx = new Acceleration(biasX,
23959                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23960         final Acceleration by = new Acceleration(biasY,
23961                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23962         final Acceleration bz = new Acceleration(biasZ,
23963                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23964 
23965         final Matrix ma = generateMaCommonAxis();
23966         final double sx = ma.getElementAt(0, 0);
23967         final double sy = ma.getElementAt(1, 1);
23968         final double sz = ma.getElementAt(2, 2);
23969 
23970         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23971         final double latitude = Math.toRadians(
23972                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23973         final double longitude = Math.toRadians(
23974                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23975         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23976         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23977         final NEDVelocity nedVelocity = new NEDVelocity();
23978         final ECEFPosition ecefPosition = new ECEFPosition();
23979         final ECEFVelocity ecefVelocity = new ECEFVelocity();
23980         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23981                 ecefPosition, ecefVelocity);
23982 
23983         final KnownPositionAccelerometerCalibrator calibrator =
23984                 new KnownPositionAccelerometerCalibrator(nedPosition,
23985                         bx, by, bz, sx, sy, sz);
23986 
23987         // check default values
23988         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23989         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23990         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23991         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23992         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23993         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23994         final Acceleration bx2 = new Acceleration(0.0,
23995                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23996         calibrator.getInitialBiasXAsAcceleration(bx2);
23997         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23998         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23999         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24000         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24001         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24002         final Acceleration by2 = new Acceleration(0.0,
24003                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24004         calibrator.getInitialBiasYAsAcceleration(by2);
24005         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24006         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24007         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24008         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24009         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24010         final Acceleration bz2 = new Acceleration(0.0,
24011                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24012         calibrator.getInitialBiasZAsAcceleration(bz2);
24013         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24014         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24015         assertEquals(calibrator.getInitialSx(), sx, 0.0);
24016         assertEquals(calibrator.getInitialSy(), sy, 0.0);
24017         assertEquals(calibrator.getInitialSz(), sz, 0.0);
24018         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24019         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24020         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24021         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24022         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24023         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24024         final double[] bias1 = calibrator.getInitialBias();
24025         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24026         final double[] bias2 = new double[3];
24027         calibrator.getInitialBias(bias2);
24028         assertArrayEquals(bias1, bias2, 0.0);
24029         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24030         assertEquals(b1, ba);
24031         final Matrix b2 = new Matrix(3, 1);
24032         calibrator.getInitialBiasAsMatrix(b2);
24033         assertEquals(b1, b2);
24034         final Matrix ma1 = calibrator.getInitialMa();
24035         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24036         final Matrix ma2 = new Matrix(3, 3);
24037         calibrator.getInitialMa(ma2);
24038         assertEquals(ma1, ma2);
24039         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24040         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24041         final NEDPosition nedPosition1 = new NEDPosition();
24042         assertTrue(calibrator.getNedPosition(nedPosition1));
24043         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24044         assertNull(calibrator.getMeasurements());
24045         assertFalse(calibrator.isCommonAxisUsed());
24046         assertNull(calibrator.getListener());
24047         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
24048         assertFalse(calibrator.isReady());
24049         assertFalse(calibrator.isRunning());
24050         assertNull(calibrator.getEstimatedBiases());
24051         assertFalse(calibrator.getEstimatedBiases(null));
24052         assertNull(calibrator.getEstimatedBiasesAsMatrix());
24053         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24054         assertNull(calibrator.getEstimatedBiasFx());
24055         assertNull(calibrator.getEstimatedBiasFy());
24056         assertNull(calibrator.getEstimatedBiasFz());
24057         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24058         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24059         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24060         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24061         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24062         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24063         assertNull(calibrator.getEstimatedMa());
24064         assertNull(calibrator.getEstimatedSx());
24065         assertNull(calibrator.getEstimatedSy());
24066         assertNull(calibrator.getEstimatedSz());
24067         assertNull(calibrator.getEstimatedMxy());
24068         assertNull(calibrator.getEstimatedMxz());
24069         assertNull(calibrator.getEstimatedMyx());
24070         assertNull(calibrator.getEstimatedMyz());
24071         assertNull(calibrator.getEstimatedMzx());
24072         assertNull(calibrator.getEstimatedMzy());
24073         assertNull(calibrator.getEstimatedCovariance());
24074         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24075         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24076                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24077         assertNotNull(calibrator.getGroundTruthGravityNorm());
24078         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24079         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24080         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24081                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24082         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24083         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24084         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24085     }
24086 
24087     @Test
24088     public void testConstructor189() throws WrongSizeException {
24089         final Matrix ba = generateBa();
24090         final double biasX = ba.getElementAtIndex(0);
24091         final double biasY = ba.getElementAtIndex(1);
24092         final double biasZ = ba.getElementAtIndex(2);
24093 
24094         final Acceleration bx = new Acceleration(biasX,
24095                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24096         final Acceleration by = new Acceleration(biasY,
24097                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24098         final Acceleration bz = new Acceleration(biasZ,
24099                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24100 
24101         final Matrix ma = generateMaCommonAxis();
24102         final double sx = ma.getElementAt(0, 0);
24103         final double sy = ma.getElementAt(1, 1);
24104         final double sz = ma.getElementAt(2, 2);
24105 
24106         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24107         final double latitude = Math.toRadians(
24108                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24109         final double longitude = Math.toRadians(
24110                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24111         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24112         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24113         final NEDVelocity nedVelocity = new NEDVelocity();
24114         final ECEFPosition ecefPosition = new ECEFPosition();
24115         final ECEFVelocity ecefVelocity = new ECEFVelocity();
24116         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24117                 ecefPosition, ecefVelocity);
24118 
24119         final KnownPositionAccelerometerCalibrator calibrator =
24120                 new KnownPositionAccelerometerCalibrator(nedPosition,
24121                         bx, by, bz, sx, sy, sz, this);
24122 
24123         // check default values
24124         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24125         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24126         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24127         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24128         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24129         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24130         final Acceleration bx2 = new Acceleration(0.0,
24131                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24132         calibrator.getInitialBiasXAsAcceleration(bx2);
24133         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24134         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24135         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24136         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24137         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24138         final Acceleration by2 = new Acceleration(0.0,
24139                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24140         calibrator.getInitialBiasYAsAcceleration(by2);
24141         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24142         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24143         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24144         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24145         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24146         final Acceleration bz2 = new Acceleration(0.0,
24147                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24148         calibrator.getInitialBiasZAsAcceleration(bz2);
24149         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24150         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24151         assertEquals(calibrator.getInitialSx(), sx, 0.0);
24152         assertEquals(calibrator.getInitialSy(), sy, 0.0);
24153         assertEquals(calibrator.getInitialSz(), sz, 0.0);
24154         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24155         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24156         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24157         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24158         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24159         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24160         final double[] bias1 = calibrator.getInitialBias();
24161         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24162         final double[] bias2 = new double[3];
24163         calibrator.getInitialBias(bias2);
24164         assertArrayEquals(bias1, bias2, 0.0);
24165         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24166         assertEquals(b1, ba);
24167         final Matrix b2 = new Matrix(3, 1);
24168         calibrator.getInitialBiasAsMatrix(b2);
24169         assertEquals(b1, b2);
24170         final Matrix ma1 = calibrator.getInitialMa();
24171         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24172         final Matrix ma2 = new Matrix(3, 3);
24173         calibrator.getInitialMa(ma2);
24174         assertEquals(ma1, ma2);
24175         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24176         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24177         final NEDPosition nedPosition1 = new NEDPosition();
24178         assertTrue(calibrator.getNedPosition(nedPosition1));
24179         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24180         assertNull(calibrator.getMeasurements());
24181         assertFalse(calibrator.isCommonAxisUsed());
24182         assertSame(calibrator.getListener(), this);
24183         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
24184         assertFalse(calibrator.isReady());
24185         assertFalse(calibrator.isRunning());
24186         assertNull(calibrator.getEstimatedBiases());
24187         assertFalse(calibrator.getEstimatedBiases(null));
24188         assertNull(calibrator.getEstimatedBiasesAsMatrix());
24189         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24190         assertNull(calibrator.getEstimatedBiasFx());
24191         assertNull(calibrator.getEstimatedBiasFy());
24192         assertNull(calibrator.getEstimatedBiasFz());
24193         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24194         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24195         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24196         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24197         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24198         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24199         assertNull(calibrator.getEstimatedMa());
24200         assertNull(calibrator.getEstimatedSx());
24201         assertNull(calibrator.getEstimatedSy());
24202         assertNull(calibrator.getEstimatedSz());
24203         assertNull(calibrator.getEstimatedMxy());
24204         assertNull(calibrator.getEstimatedMxz());
24205         assertNull(calibrator.getEstimatedMyx());
24206         assertNull(calibrator.getEstimatedMyz());
24207         assertNull(calibrator.getEstimatedMzx());
24208         assertNull(calibrator.getEstimatedMzy());
24209         assertNull(calibrator.getEstimatedCovariance());
24210         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24211         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24212                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24213         assertNotNull(calibrator.getGroundTruthGravityNorm());
24214         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24215         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24216         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24217                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24218         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24219         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24220         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24221     }
24222 
24223     @Test
24224     public void testConstructor190() throws WrongSizeException {
24225         final Collection<StandardDeviationBodyKinematics> measurements =
24226                 Collections.emptyList();
24227 
24228         final Matrix ba = generateBa();
24229         final double biasX = ba.getElementAtIndex(0);
24230         final double biasY = ba.getElementAtIndex(1);
24231         final double biasZ = ba.getElementAtIndex(2);
24232 
24233         final Acceleration bx = new Acceleration(biasX,
24234                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24235         final Acceleration by = new Acceleration(biasY,
24236                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24237         final Acceleration bz = new Acceleration(biasZ,
24238                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24239 
24240         final Matrix ma = generateMaCommonAxis();
24241         final double sx = ma.getElementAt(0, 0);
24242         final double sy = ma.getElementAt(1, 1);
24243         final double sz = ma.getElementAt(2, 2);
24244 
24245         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24246         final double latitude = Math.toRadians(
24247                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24248         final double longitude = Math.toRadians(
24249                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24250         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24251         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24252         final NEDVelocity nedVelocity = new NEDVelocity();
24253         final ECEFPosition ecefPosition = new ECEFPosition();
24254         final ECEFVelocity ecefVelocity = new ECEFVelocity();
24255         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24256                 ecefPosition, ecefVelocity);
24257 
24258         final KnownPositionAccelerometerCalibrator calibrator =
24259                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
24260                         bx, by, bz, sx, sy, sz);
24261 
24262         // check default values
24263         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24264         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24265         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24266         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24267         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24268         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24269         final Acceleration bx2 = new Acceleration(0.0,
24270                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24271         calibrator.getInitialBiasXAsAcceleration(bx2);
24272         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24273         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24274         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24275         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24276         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24277         final Acceleration by2 = new Acceleration(0.0,
24278                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24279         calibrator.getInitialBiasYAsAcceleration(by2);
24280         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24281         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24282         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24283         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24284         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24285         final Acceleration bz2 = new Acceleration(0.0,
24286                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24287         calibrator.getInitialBiasZAsAcceleration(bz2);
24288         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24289         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24290         assertEquals(calibrator.getInitialSx(), sx, 0.0);
24291         assertEquals(calibrator.getInitialSy(), sy, 0.0);
24292         assertEquals(calibrator.getInitialSz(), sz, 0.0);
24293         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24294         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24295         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24296         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24297         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24298         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24299         final double[] bias1 = calibrator.getInitialBias();
24300         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24301         final double[] bias2 = new double[3];
24302         calibrator.getInitialBias(bias2);
24303         assertArrayEquals(bias1, bias2, 0.0);
24304         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24305         assertEquals(b1, ba);
24306         final Matrix b2 = new Matrix(3, 1);
24307         calibrator.getInitialBiasAsMatrix(b2);
24308         assertEquals(b1, b2);
24309         final Matrix ma1 = calibrator.getInitialMa();
24310         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24311         final Matrix ma2 = new Matrix(3, 3);
24312         calibrator.getInitialMa(ma2);
24313         assertEquals(ma1, ma2);
24314         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24315         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24316         final NEDPosition nedPosition1 = new NEDPosition();
24317         assertTrue(calibrator.getNedPosition(nedPosition1));
24318         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24319         assertSame(calibrator.getMeasurements(), measurements);
24320         assertFalse(calibrator.isCommonAxisUsed());
24321         assertNull(calibrator.getListener());
24322         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
24323         assertFalse(calibrator.isReady());
24324         assertFalse(calibrator.isRunning());
24325         assertNull(calibrator.getEstimatedBiases());
24326         assertFalse(calibrator.getEstimatedBiases(null));
24327         assertNull(calibrator.getEstimatedBiasesAsMatrix());
24328         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24329         assertNull(calibrator.getEstimatedBiasFx());
24330         assertNull(calibrator.getEstimatedBiasFy());
24331         assertNull(calibrator.getEstimatedBiasFz());
24332         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24333         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24334         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24335         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24336         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24337         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24338         assertNull(calibrator.getEstimatedMa());
24339         assertNull(calibrator.getEstimatedSx());
24340         assertNull(calibrator.getEstimatedSy());
24341         assertNull(calibrator.getEstimatedSz());
24342         assertNull(calibrator.getEstimatedMxy());
24343         assertNull(calibrator.getEstimatedMxz());
24344         assertNull(calibrator.getEstimatedMyx());
24345         assertNull(calibrator.getEstimatedMyz());
24346         assertNull(calibrator.getEstimatedMzx());
24347         assertNull(calibrator.getEstimatedMzy());
24348         assertNull(calibrator.getEstimatedCovariance());
24349         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24350         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24351                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24352         assertNotNull(calibrator.getGroundTruthGravityNorm());
24353         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24354         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24355         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24356                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24357         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24358         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24359         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24360     }
24361 
24362     @Test
24363     public void testConstructor191() throws WrongSizeException {
24364         final Collection<StandardDeviationBodyKinematics> measurements =
24365                 Collections.emptyList();
24366 
24367         final Matrix ba = generateBa();
24368         final double biasX = ba.getElementAtIndex(0);
24369         final double biasY = ba.getElementAtIndex(1);
24370         final double biasZ = ba.getElementAtIndex(2);
24371 
24372         final Acceleration bx = new Acceleration(biasX,
24373                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24374         final Acceleration by = new Acceleration(biasY,
24375                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24376         final Acceleration bz = new Acceleration(biasZ,
24377                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24378 
24379         final Matrix ma = generateMaCommonAxis();
24380         final double sx = ma.getElementAt(0, 0);
24381         final double sy = ma.getElementAt(1, 1);
24382         final double sz = ma.getElementAt(2, 2);
24383 
24384         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24385         final double latitude = Math.toRadians(
24386                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24387         final double longitude = Math.toRadians(
24388                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24389         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24390         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24391         final NEDVelocity nedVelocity = new NEDVelocity();
24392         final ECEFPosition ecefPosition = new ECEFPosition();
24393         final ECEFVelocity ecefVelocity = new ECEFVelocity();
24394         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24395                 ecefPosition, ecefVelocity);
24396 
24397         final KnownPositionAccelerometerCalibrator calibrator =
24398                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
24399                         bx, by, bz, sx, sy, sz, this);
24400 
24401         // check default values
24402         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24403         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24404         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24405         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24406         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24407         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24408         final Acceleration bx2 = new Acceleration(0.0,
24409                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24410         calibrator.getInitialBiasXAsAcceleration(bx2);
24411         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24412         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24413         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24414         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24415         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24416         final Acceleration by2 = new Acceleration(0.0,
24417                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24418         calibrator.getInitialBiasYAsAcceleration(by2);
24419         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24420         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24421         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24422         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24423         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24424         final Acceleration bz2 = new Acceleration(0.0,
24425                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24426         calibrator.getInitialBiasZAsAcceleration(bz2);
24427         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24428         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24429         assertEquals(calibrator.getInitialSx(), sx, 0.0);
24430         assertEquals(calibrator.getInitialSy(), sy, 0.0);
24431         assertEquals(calibrator.getInitialSz(), sz, 0.0);
24432         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24433         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24434         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24435         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24436         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24437         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24438         final double[] bias1 = calibrator.getInitialBias();
24439         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24440         final double[] bias2 = new double[3];
24441         calibrator.getInitialBias(bias2);
24442         assertArrayEquals(bias1, bias2, 0.0);
24443         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24444         assertEquals(b1, ba);
24445         final Matrix b2 = new Matrix(3, 1);
24446         calibrator.getInitialBiasAsMatrix(b2);
24447         assertEquals(b1, b2);
24448         final Matrix ma1 = calibrator.getInitialMa();
24449         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24450         final Matrix ma2 = new Matrix(3, 3);
24451         calibrator.getInitialMa(ma2);
24452         assertEquals(ma1, ma2);
24453         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24454         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24455         final NEDPosition nedPosition1 = new NEDPosition();
24456         assertTrue(calibrator.getNedPosition(nedPosition1));
24457         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24458         assertSame(calibrator.getMeasurements(), measurements);
24459         assertFalse(calibrator.isCommonAxisUsed());
24460         assertSame(calibrator.getListener(), this);
24461         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
24462         assertFalse(calibrator.isReady());
24463         assertFalse(calibrator.isRunning());
24464         assertNull(calibrator.getEstimatedBiases());
24465         assertFalse(calibrator.getEstimatedBiases(null));
24466         assertNull(calibrator.getEstimatedBiasesAsMatrix());
24467         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24468         assertNull(calibrator.getEstimatedBiasFx());
24469         assertNull(calibrator.getEstimatedBiasFy());
24470         assertNull(calibrator.getEstimatedBiasFz());
24471         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24472         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24473         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24474         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24475         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24476         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24477         assertNull(calibrator.getEstimatedMa());
24478         assertNull(calibrator.getEstimatedSx());
24479         assertNull(calibrator.getEstimatedSy());
24480         assertNull(calibrator.getEstimatedSz());
24481         assertNull(calibrator.getEstimatedMxy());
24482         assertNull(calibrator.getEstimatedMxz());
24483         assertNull(calibrator.getEstimatedMyx());
24484         assertNull(calibrator.getEstimatedMyz());
24485         assertNull(calibrator.getEstimatedMzx());
24486         assertNull(calibrator.getEstimatedMzy());
24487         assertNull(calibrator.getEstimatedCovariance());
24488         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24489         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24490                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24491         assertNotNull(calibrator.getGroundTruthGravityNorm());
24492         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24493         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24494         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24495                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24496         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24497         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24498         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24499     }
24500 
24501     @Test
24502     public void testConstructor192() throws WrongSizeException {
24503         final Matrix ba = generateBa();
24504         final double biasX = ba.getElementAtIndex(0);
24505         final double biasY = ba.getElementAtIndex(1);
24506         final double biasZ = ba.getElementAtIndex(2);
24507 
24508         final Acceleration bx = new Acceleration(biasX,
24509                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24510         final Acceleration by = new Acceleration(biasY,
24511                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24512         final Acceleration bz = new Acceleration(biasZ,
24513                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24514 
24515         final Matrix ma = generateMaCommonAxis();
24516         final double sx = ma.getElementAt(0, 0);
24517         final double sy = ma.getElementAt(1, 1);
24518         final double sz = ma.getElementAt(2, 2);
24519 
24520         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24521         final double latitude = Math.toRadians(
24522                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24523         final double longitude = Math.toRadians(
24524                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24525         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24526         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24527         final NEDVelocity nedVelocity = new NEDVelocity();
24528         final ECEFPosition ecefPosition = new ECEFPosition();
24529         final ECEFVelocity ecefVelocity = new ECEFVelocity();
24530         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24531                 ecefPosition, ecefVelocity);
24532 
24533         final KnownPositionAccelerometerCalibrator calibrator =
24534                 new KnownPositionAccelerometerCalibrator(nedPosition,
24535                         true, bx, by, bz, sx, sy, sz);
24536 
24537         // check default values
24538         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24539         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24540         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24541         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24542         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24543         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24544         final Acceleration bx2 = new Acceleration(0.0,
24545                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24546         calibrator.getInitialBiasXAsAcceleration(bx2);
24547         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24548         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24549         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24550         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24551         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24552         final Acceleration by2 = new Acceleration(0.0,
24553                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24554         calibrator.getInitialBiasYAsAcceleration(by2);
24555         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24556         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24557         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24558         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24559         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24560         final Acceleration bz2 = new Acceleration(0.0,
24561                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24562         calibrator.getInitialBiasZAsAcceleration(bz2);
24563         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24564         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24565         assertEquals(calibrator.getInitialSx(), sx, 0.0);
24566         assertEquals(calibrator.getInitialSy(), sy, 0.0);
24567         assertEquals(calibrator.getInitialSz(), sz, 0.0);
24568         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24569         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24570         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24571         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24572         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24573         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24574         final double[] bias1 = calibrator.getInitialBias();
24575         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24576         final double[] bias2 = new double[3];
24577         calibrator.getInitialBias(bias2);
24578         assertArrayEquals(bias1, bias2, 0.0);
24579         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24580         assertEquals(b1, ba);
24581         final Matrix b2 = new Matrix(3, 1);
24582         calibrator.getInitialBiasAsMatrix(b2);
24583         assertEquals(b1, b2);
24584         final Matrix ma1 = calibrator.getInitialMa();
24585         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24586         final Matrix ma2 = new Matrix(3, 3);
24587         calibrator.getInitialMa(ma2);
24588         assertEquals(ma1, ma2);
24589         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24590         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24591         final NEDPosition nedPosition1 = new NEDPosition();
24592         assertTrue(calibrator.getNedPosition(nedPosition1));
24593         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24594         assertNull(calibrator.getMeasurements());
24595         assertTrue(calibrator.isCommonAxisUsed());
24596         assertNull(calibrator.getListener());
24597         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
24598         assertFalse(calibrator.isReady());
24599         assertFalse(calibrator.isRunning());
24600         assertNull(calibrator.getEstimatedBiases());
24601         assertFalse(calibrator.getEstimatedBiases(null));
24602         assertNull(calibrator.getEstimatedBiasesAsMatrix());
24603         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24604         assertNull(calibrator.getEstimatedBiasFx());
24605         assertNull(calibrator.getEstimatedBiasFy());
24606         assertNull(calibrator.getEstimatedBiasFz());
24607         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24608         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24609         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24610         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24611         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24612         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24613         assertNull(calibrator.getEstimatedMa());
24614         assertNull(calibrator.getEstimatedSx());
24615         assertNull(calibrator.getEstimatedSy());
24616         assertNull(calibrator.getEstimatedSz());
24617         assertNull(calibrator.getEstimatedMxy());
24618         assertNull(calibrator.getEstimatedMxz());
24619         assertNull(calibrator.getEstimatedMyx());
24620         assertNull(calibrator.getEstimatedMyz());
24621         assertNull(calibrator.getEstimatedMzx());
24622         assertNull(calibrator.getEstimatedMzy());
24623         assertNull(calibrator.getEstimatedCovariance());
24624         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24625         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24626                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24627         assertNotNull(calibrator.getGroundTruthGravityNorm());
24628         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24629         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24630         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24631                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24632         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24633         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24634         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24635     }
24636 
24637     @Test
24638     public void testConstructor193() throws WrongSizeException {
24639         final Matrix ba = generateBa();
24640         final double biasX = ba.getElementAtIndex(0);
24641         final double biasY = ba.getElementAtIndex(1);
24642         final double biasZ = ba.getElementAtIndex(2);
24643 
24644         final Acceleration bx = new Acceleration(biasX,
24645                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24646         final Acceleration by = new Acceleration(biasY,
24647                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24648         final Acceleration bz = new Acceleration(biasZ,
24649                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24650 
24651         final Matrix ma = generateMaCommonAxis();
24652         final double sx = ma.getElementAt(0, 0);
24653         final double sy = ma.getElementAt(1, 1);
24654         final double sz = ma.getElementAt(2, 2);
24655 
24656         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24657         final double latitude = Math.toRadians(
24658                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24659         final double longitude = Math.toRadians(
24660                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24661         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24662         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24663         final NEDVelocity nedVelocity = new NEDVelocity();
24664         final ECEFPosition ecefPosition = new ECEFPosition();
24665         final ECEFVelocity ecefVelocity = new ECEFVelocity();
24666         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24667                 ecefPosition, ecefVelocity);
24668 
24669         final KnownPositionAccelerometerCalibrator calibrator =
24670                 new KnownPositionAccelerometerCalibrator(nedPosition,
24671                         true, bx, by, bz, sx, sy, sz, this);
24672 
24673         // check default values
24674         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24675         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24676         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24677         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24678         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24679         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24680         final Acceleration bx2 = new Acceleration(0.0,
24681                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24682         calibrator.getInitialBiasXAsAcceleration(bx2);
24683         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24684         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24685         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24686         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24687         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24688         final Acceleration by2 = new Acceleration(0.0,
24689                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24690         calibrator.getInitialBiasYAsAcceleration(by2);
24691         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24692         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24693         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24694         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24695         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24696         final Acceleration bz2 = new Acceleration(0.0,
24697                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24698         calibrator.getInitialBiasZAsAcceleration(bz2);
24699         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24700         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24701         assertEquals(calibrator.getInitialSx(), sx, 0.0);
24702         assertEquals(calibrator.getInitialSy(), sy, 0.0);
24703         assertEquals(calibrator.getInitialSz(), sz, 0.0);
24704         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24705         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24706         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24707         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24708         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24709         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24710         final double[] bias1 = calibrator.getInitialBias();
24711         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24712         final double[] bias2 = new double[3];
24713         calibrator.getInitialBias(bias2);
24714         assertArrayEquals(bias1, bias2, 0.0);
24715         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24716         assertEquals(b1, ba);
24717         final Matrix b2 = new Matrix(3, 1);
24718         calibrator.getInitialBiasAsMatrix(b2);
24719         assertEquals(b1, b2);
24720         final Matrix ma1 = calibrator.getInitialMa();
24721         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24722         final Matrix ma2 = new Matrix(3, 3);
24723         calibrator.getInitialMa(ma2);
24724         assertEquals(ma1, ma2);
24725         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24726         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24727         final NEDPosition nedPosition1 = new NEDPosition();
24728         assertTrue(calibrator.getNedPosition(nedPosition1));
24729         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24730         assertNull(calibrator.getMeasurements());
24731         assertTrue(calibrator.isCommonAxisUsed());
24732         assertSame(calibrator.getListener(), this);
24733         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
24734         assertFalse(calibrator.isReady());
24735         assertFalse(calibrator.isRunning());
24736         assertNull(calibrator.getEstimatedBiases());
24737         assertFalse(calibrator.getEstimatedBiases(null));
24738         assertNull(calibrator.getEstimatedBiasesAsMatrix());
24739         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24740         assertNull(calibrator.getEstimatedBiasFx());
24741         assertNull(calibrator.getEstimatedBiasFy());
24742         assertNull(calibrator.getEstimatedBiasFz());
24743         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24744         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24745         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24746         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24747         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24748         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24749         assertNull(calibrator.getEstimatedMa());
24750         assertNull(calibrator.getEstimatedSx());
24751         assertNull(calibrator.getEstimatedSy());
24752         assertNull(calibrator.getEstimatedSz());
24753         assertNull(calibrator.getEstimatedMxy());
24754         assertNull(calibrator.getEstimatedMxz());
24755         assertNull(calibrator.getEstimatedMyx());
24756         assertNull(calibrator.getEstimatedMyz());
24757         assertNull(calibrator.getEstimatedMzx());
24758         assertNull(calibrator.getEstimatedMzy());
24759         assertNull(calibrator.getEstimatedCovariance());
24760         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24761         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24762                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24763         assertNotNull(calibrator.getGroundTruthGravityNorm());
24764         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24765         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24766         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24767                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24768         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24769         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24770         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24771     }
24772 
24773     @Test
24774     public void testConstructor194() throws WrongSizeException {
24775         final Collection<StandardDeviationBodyKinematics> measurements =
24776                 Collections.emptyList();
24777 
24778         final Matrix ba = generateBa();
24779         final double biasX = ba.getElementAtIndex(0);
24780         final double biasY = ba.getElementAtIndex(1);
24781         final double biasZ = ba.getElementAtIndex(2);
24782 
24783         final Acceleration bx = new Acceleration(biasX,
24784                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24785         final Acceleration by = new Acceleration(biasY,
24786                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24787         final Acceleration bz = new Acceleration(biasZ,
24788                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24789 
24790         final Matrix ma = generateMaCommonAxis();
24791         final double sx = ma.getElementAt(0, 0);
24792         final double sy = ma.getElementAt(1, 1);
24793         final double sz = ma.getElementAt(2, 2);
24794 
24795         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24796         final double latitude = Math.toRadians(
24797                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24798         final double longitude = Math.toRadians(
24799                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24800         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24801         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24802         final NEDVelocity nedVelocity = new NEDVelocity();
24803         final ECEFPosition ecefPosition = new ECEFPosition();
24804         final ECEFVelocity ecefVelocity = new ECEFVelocity();
24805         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24806                 ecefPosition, ecefVelocity);
24807 
24808         final KnownPositionAccelerometerCalibrator calibrator =
24809                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
24810                         true, bx, by, bz, sx, sy, sz);
24811 
24812         // check default values
24813         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24814         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24815         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24816         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24817         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24818         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24819         final Acceleration bx2 = new Acceleration(0.0,
24820                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24821         calibrator.getInitialBiasXAsAcceleration(bx2);
24822         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24823         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24824         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24825         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24826         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24827         final Acceleration by2 = new Acceleration(0.0,
24828                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24829         calibrator.getInitialBiasYAsAcceleration(by2);
24830         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24831         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24832         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24833         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24834         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24835         final Acceleration bz2 = new Acceleration(0.0,
24836                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24837         calibrator.getInitialBiasZAsAcceleration(bz2);
24838         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24839         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24840         assertEquals(calibrator.getInitialSx(), sx, 0.0);
24841         assertEquals(calibrator.getInitialSy(), sy, 0.0);
24842         assertEquals(calibrator.getInitialSz(), sz, 0.0);
24843         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24844         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24845         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24846         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24847         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24848         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24849         final double[] bias1 = calibrator.getInitialBias();
24850         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24851         final double[] bias2 = new double[3];
24852         calibrator.getInitialBias(bias2);
24853         assertArrayEquals(bias1, bias2, 0.0);
24854         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24855         assertEquals(b1, ba);
24856         final Matrix b2 = new Matrix(3, 1);
24857         calibrator.getInitialBiasAsMatrix(b2);
24858         assertEquals(b1, b2);
24859         final Matrix ma1 = calibrator.getInitialMa();
24860         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24861         final Matrix ma2 = new Matrix(3, 3);
24862         calibrator.getInitialMa(ma2);
24863         assertEquals(ma1, ma2);
24864         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24865         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24866         final NEDPosition nedPosition1 = new NEDPosition();
24867         assertTrue(calibrator.getNedPosition(nedPosition1));
24868         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24869         assertSame(calibrator.getMeasurements(), measurements);
24870         assertTrue(calibrator.isCommonAxisUsed());
24871         assertNull(calibrator.getListener());
24872         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
24873         assertFalse(calibrator.isReady());
24874         assertFalse(calibrator.isRunning());
24875         assertNull(calibrator.getEstimatedBiases());
24876         assertFalse(calibrator.getEstimatedBiases(null));
24877         assertNull(calibrator.getEstimatedBiasesAsMatrix());
24878         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24879         assertNull(calibrator.getEstimatedBiasFx());
24880         assertNull(calibrator.getEstimatedBiasFy());
24881         assertNull(calibrator.getEstimatedBiasFz());
24882         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24883         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24884         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24885         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24886         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24887         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24888         assertNull(calibrator.getEstimatedMa());
24889         assertNull(calibrator.getEstimatedSx());
24890         assertNull(calibrator.getEstimatedSy());
24891         assertNull(calibrator.getEstimatedSz());
24892         assertNull(calibrator.getEstimatedMxy());
24893         assertNull(calibrator.getEstimatedMxz());
24894         assertNull(calibrator.getEstimatedMyx());
24895         assertNull(calibrator.getEstimatedMyz());
24896         assertNull(calibrator.getEstimatedMzx());
24897         assertNull(calibrator.getEstimatedMzy());
24898         assertNull(calibrator.getEstimatedCovariance());
24899         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24900         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24901                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24902         assertNotNull(calibrator.getGroundTruthGravityNorm());
24903         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24904         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24905         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24906                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24907         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24908         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24909         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24910     }
24911 
24912     @Test
24913     public void testConstructor195() throws WrongSizeException {
24914         final Collection<StandardDeviationBodyKinematics> measurements =
24915                 Collections.emptyList();
24916 
24917         final Matrix ba = generateBa();
24918         final double biasX = ba.getElementAtIndex(0);
24919         final double biasY = ba.getElementAtIndex(1);
24920         final double biasZ = ba.getElementAtIndex(2);
24921 
24922         final Acceleration bx = new Acceleration(biasX,
24923                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24924         final Acceleration by = new Acceleration(biasY,
24925                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24926         final Acceleration bz = new Acceleration(biasZ,
24927                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24928 
24929         final Matrix ma = generateMaCommonAxis();
24930         final double sx = ma.getElementAt(0, 0);
24931         final double sy = ma.getElementAt(1, 1);
24932         final double sz = ma.getElementAt(2, 2);
24933 
24934         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24935         final double latitude = Math.toRadians(
24936                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24937         final double longitude = Math.toRadians(
24938                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24939         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24940         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24941         final NEDVelocity nedVelocity = new NEDVelocity();
24942         final ECEFPosition ecefPosition = new ECEFPosition();
24943         final ECEFVelocity ecefVelocity = new ECEFVelocity();
24944         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24945                 ecefPosition, ecefVelocity);
24946 
24947         final KnownPositionAccelerometerCalibrator calibrator =
24948                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
24949                         true, bx, by, bz, sx, sy, sz, this);
24950 
24951         // check default values
24952         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24953         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24954         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24955         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24956         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24957         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24958         final Acceleration bx2 = new Acceleration(0.0,
24959                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24960         calibrator.getInitialBiasXAsAcceleration(bx2);
24961         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24962         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24963         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24964         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24965         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24966         final Acceleration by2 = new Acceleration(0.0,
24967                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24968         calibrator.getInitialBiasYAsAcceleration(by2);
24969         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24970         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24971         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24972         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24973         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24974         final Acceleration bz2 = new Acceleration(0.0,
24975                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24976         calibrator.getInitialBiasZAsAcceleration(bz2);
24977         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24978         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24979         assertEquals(calibrator.getInitialSx(), sx, 0.0);
24980         assertEquals(calibrator.getInitialSy(), sy, 0.0);
24981         assertEquals(calibrator.getInitialSz(), sz, 0.0);
24982         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24983         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24984         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24985         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24986         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24987         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24988         final double[] bias1 = calibrator.getInitialBias();
24989         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24990         final double[] bias2 = new double[3];
24991         calibrator.getInitialBias(bias2);
24992         assertArrayEquals(bias1, bias2, 0.0);
24993         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24994         assertEquals(b1, ba);
24995         final Matrix b2 = new Matrix(3, 1);
24996         calibrator.getInitialBiasAsMatrix(b2);
24997         assertEquals(b1, b2);
24998         final Matrix ma1 = calibrator.getInitialMa();
24999         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
25000         final Matrix ma2 = new Matrix(3, 3);
25001         calibrator.getInitialMa(ma2);
25002         assertEquals(ma1, ma2);
25003         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25004         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25005         final NEDPosition nedPosition1 = new NEDPosition();
25006         assertTrue(calibrator.getNedPosition(nedPosition1));
25007         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25008         assertSame(calibrator.getMeasurements(), measurements);
25009         assertTrue(calibrator.isCommonAxisUsed());
25010         assertSame(calibrator.getListener(), this);
25011         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
25012         assertFalse(calibrator.isReady());
25013         assertFalse(calibrator.isRunning());
25014         assertNull(calibrator.getEstimatedBiases());
25015         assertFalse(calibrator.getEstimatedBiases(null));
25016         assertNull(calibrator.getEstimatedBiasesAsMatrix());
25017         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25018         assertNull(calibrator.getEstimatedBiasFx());
25019         assertNull(calibrator.getEstimatedBiasFy());
25020         assertNull(calibrator.getEstimatedBiasFz());
25021         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25022         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25023         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25024         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25025         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25026         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25027         assertNull(calibrator.getEstimatedMa());
25028         assertNull(calibrator.getEstimatedSx());
25029         assertNull(calibrator.getEstimatedSy());
25030         assertNull(calibrator.getEstimatedSz());
25031         assertNull(calibrator.getEstimatedMxy());
25032         assertNull(calibrator.getEstimatedMxz());
25033         assertNull(calibrator.getEstimatedMyx());
25034         assertNull(calibrator.getEstimatedMyz());
25035         assertNull(calibrator.getEstimatedMzx());
25036         assertNull(calibrator.getEstimatedMzy());
25037         assertNull(calibrator.getEstimatedCovariance());
25038         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25039         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25040                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25041         assertNotNull(calibrator.getGroundTruthGravityNorm());
25042         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25043         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25044         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25045                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25046         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25047         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25048         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25049     }
25050 
25051     @Test
25052     public void testConstructor196() throws WrongSizeException {
25053         final Matrix ba = generateBa();
25054         final double biasX = ba.getElementAtIndex(0);
25055         final double biasY = ba.getElementAtIndex(1);
25056         final double biasZ = ba.getElementAtIndex(2);
25057 
25058         final Matrix ma = generateMaCommonAxis();
25059         final double sx = ma.getElementAt(0, 0);
25060         final double sy = ma.getElementAt(1, 1);
25061         final double sz = ma.getElementAt(2, 2);
25062         final double mxy = ma.getElementAt(0, 1);
25063         final double mxz = ma.getElementAt(0, 2);
25064         final double myx = ma.getElementAt(1, 0);
25065         final double myz = ma.getElementAt(1, 2);
25066         final double mzx = ma.getElementAt(2, 0);
25067         final double mzy = ma.getElementAt(2, 1);
25068 
25069         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25070         final double latitude = Math.toRadians(
25071                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25072         final double longitude = Math.toRadians(
25073                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25074         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25075         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25076         final NEDVelocity nedVelocity = new NEDVelocity();
25077         final ECEFPosition ecefPosition = new ECEFPosition();
25078         final ECEFVelocity ecefVelocity = new ECEFVelocity();
25079         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25080                 ecefPosition, ecefVelocity);
25081 
25082         final KnownPositionAccelerometerCalibrator calibrator =
25083                 new KnownPositionAccelerometerCalibrator(nedPosition,
25084                         biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
25085                         myx, myz, mzx, mzy);
25086 
25087         // check default values
25088         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25089         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25090         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25091         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25092         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25093         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25094         final Acceleration bx2 = new Acceleration(0.0,
25095                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25096         calibrator.getInitialBiasXAsAcceleration(bx2);
25097         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25098         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25099         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25100         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25101         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25102         final Acceleration by2 = new Acceleration(0.0,
25103                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25104         calibrator.getInitialBiasYAsAcceleration(by2);
25105         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25106         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25107         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25108         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25109         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25110         final Acceleration bz2 = new Acceleration(0.0,
25111                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25112         calibrator.getInitialBiasZAsAcceleration(bz2);
25113         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25114         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25115         assertEquals(calibrator.getInitialSx(), sx, 0.0);
25116         assertEquals(calibrator.getInitialSy(), sy, 0.0);
25117         assertEquals(calibrator.getInitialSz(), sz, 0.0);
25118         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25119         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25120         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25121         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25122         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25123         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25124         final double[] bias1 = calibrator.getInitialBias();
25125         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25126         final double[] bias2 = new double[3];
25127         calibrator.getInitialBias(bias2);
25128         assertArrayEquals(bias1, bias2, 0.0);
25129         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25130         assertEquals(b1, ba);
25131         final Matrix b2 = new Matrix(3, 1);
25132         calibrator.getInitialBiasAsMatrix(b2);
25133         assertEquals(b1, b2);
25134         final Matrix ma1 = new Matrix(3, 3);
25135         ma1.setSubmatrix(0, 0,
25136                 2, 2,
25137                 new double[]{sx, myx, mzx,
25138                         mxy, sy, mzy,
25139                         mxz, myz, sz});
25140         assertEquals(calibrator.getInitialMa(), ma1);
25141         final Matrix ma2 = new Matrix(3, 3);
25142         calibrator.getInitialMa(ma2);
25143         assertEquals(ma1, ma2);
25144         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25145         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25146         final NEDPosition nedPosition1 = new NEDPosition();
25147         assertTrue(calibrator.getNedPosition(nedPosition1));
25148         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25149         assertNull(calibrator.getMeasurements());
25150         assertFalse(calibrator.isCommonAxisUsed());
25151         assertNull(calibrator.getListener());
25152         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
25153         assertFalse(calibrator.isReady());
25154         assertFalse(calibrator.isRunning());
25155         assertNull(calibrator.getEstimatedBiases());
25156         assertFalse(calibrator.getEstimatedBiases(null));
25157         assertNull(calibrator.getEstimatedBiasesAsMatrix());
25158         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25159         assertNull(calibrator.getEstimatedBiasFx());
25160         assertNull(calibrator.getEstimatedBiasFy());
25161         assertNull(calibrator.getEstimatedBiasFz());
25162         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25163         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25164         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25165         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25166         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25167         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25168         assertNull(calibrator.getEstimatedMa());
25169         assertNull(calibrator.getEstimatedSx());
25170         assertNull(calibrator.getEstimatedSy());
25171         assertNull(calibrator.getEstimatedSz());
25172         assertNull(calibrator.getEstimatedMxy());
25173         assertNull(calibrator.getEstimatedMxz());
25174         assertNull(calibrator.getEstimatedMyx());
25175         assertNull(calibrator.getEstimatedMyz());
25176         assertNull(calibrator.getEstimatedMzx());
25177         assertNull(calibrator.getEstimatedMzy());
25178         assertNull(calibrator.getEstimatedCovariance());
25179         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25180         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25181                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25182         assertNotNull(calibrator.getGroundTruthGravityNorm());
25183         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25184         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25185         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25186                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25187         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25188         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25189         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25190     }
25191 
25192     @Test
25193     public void testConstructor197() throws WrongSizeException {
25194         final Collection<StandardDeviationBodyKinematics> measurements =
25195                 Collections.emptyList();
25196 
25197         final Matrix ba = generateBa();
25198         final double biasX = ba.getElementAtIndex(0);
25199         final double biasY = ba.getElementAtIndex(1);
25200         final double biasZ = ba.getElementAtIndex(2);
25201 
25202         final Matrix ma = generateMaCommonAxis();
25203         final double sx = ma.getElementAt(0, 0);
25204         final double sy = ma.getElementAt(1, 1);
25205         final double sz = ma.getElementAt(2, 2);
25206         final double mxy = ma.getElementAt(0, 1);
25207         final double mxz = ma.getElementAt(0, 2);
25208         final double myx = ma.getElementAt(1, 0);
25209         final double myz = ma.getElementAt(1, 2);
25210         final double mzx = ma.getElementAt(2, 0);
25211         final double mzy = ma.getElementAt(2, 1);
25212 
25213         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25214         final double latitude = Math.toRadians(
25215                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25216         final double longitude = Math.toRadians(
25217                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25218         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25219         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25220         final NEDVelocity nedVelocity = new NEDVelocity();
25221         final ECEFPosition ecefPosition = new ECEFPosition();
25222         final ECEFVelocity ecefVelocity = new ECEFVelocity();
25223         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25224                 ecefPosition, ecefVelocity);
25225 
25226         final KnownPositionAccelerometerCalibrator calibrator =
25227                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
25228                         biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
25229                         myx, myz, mzx, mzy);
25230 
25231         // check default values
25232         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25233         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25234         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25235         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25236         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25237         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25238         final Acceleration bx2 = new Acceleration(0.0,
25239                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25240         calibrator.getInitialBiasXAsAcceleration(bx2);
25241         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25242         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25243         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25244         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25245         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25246         final Acceleration by2 = new Acceleration(0.0,
25247                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25248         calibrator.getInitialBiasYAsAcceleration(by2);
25249         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25250         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25251         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25252         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25253         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25254         final Acceleration bz2 = new Acceleration(0.0,
25255                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25256         calibrator.getInitialBiasZAsAcceleration(bz2);
25257         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25258         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25259         assertEquals(calibrator.getInitialSx(), sx, 0.0);
25260         assertEquals(calibrator.getInitialSy(), sy, 0.0);
25261         assertEquals(calibrator.getInitialSz(), sz, 0.0);
25262         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25263         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25264         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25265         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25266         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25267         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25268         final double[] bias1 = calibrator.getInitialBias();
25269         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25270         final double[] bias2 = new double[3];
25271         calibrator.getInitialBias(bias2);
25272         assertArrayEquals(bias1, bias2, 0.0);
25273         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25274         assertEquals(b1, ba);
25275         final Matrix b2 = new Matrix(3, 1);
25276         calibrator.getInitialBiasAsMatrix(b2);
25277         assertEquals(b1, b2);
25278         final Matrix ma1 = new Matrix(3, 3);
25279         ma1.setSubmatrix(0, 0,
25280                 2, 2,
25281                 new double[]{sx, myx, mzx,
25282                         mxy, sy, mzy,
25283                         mxz, myz, sz});
25284         assertEquals(calibrator.getInitialMa(), ma1);
25285         final Matrix ma2 = new Matrix(3, 3);
25286         calibrator.getInitialMa(ma2);
25287         assertEquals(ma1, ma2);
25288         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25289         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25290         final NEDPosition nedPosition1 = new NEDPosition();
25291         assertTrue(calibrator.getNedPosition(nedPosition1));
25292         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25293         assertSame(calibrator.getMeasurements(), measurements);
25294         assertFalse(calibrator.isCommonAxisUsed());
25295         assertNull(calibrator.getListener());
25296         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
25297         assertFalse(calibrator.isReady());
25298         assertFalse(calibrator.isRunning());
25299         assertNull(calibrator.getEstimatedBiases());
25300         assertFalse(calibrator.getEstimatedBiases(null));
25301         assertNull(calibrator.getEstimatedBiasesAsMatrix());
25302         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25303         assertNull(calibrator.getEstimatedBiasFx());
25304         assertNull(calibrator.getEstimatedBiasFy());
25305         assertNull(calibrator.getEstimatedBiasFz());
25306         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25307         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25308         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25309         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25310         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25311         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25312         assertNull(calibrator.getEstimatedMa());
25313         assertNull(calibrator.getEstimatedSx());
25314         assertNull(calibrator.getEstimatedSy());
25315         assertNull(calibrator.getEstimatedSz());
25316         assertNull(calibrator.getEstimatedMxy());
25317         assertNull(calibrator.getEstimatedMxz());
25318         assertNull(calibrator.getEstimatedMyx());
25319         assertNull(calibrator.getEstimatedMyz());
25320         assertNull(calibrator.getEstimatedMzx());
25321         assertNull(calibrator.getEstimatedMzy());
25322         assertNull(calibrator.getEstimatedCovariance());
25323         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25324         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25325                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25326         assertNotNull(calibrator.getGroundTruthGravityNorm());
25327         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25328         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25329         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25330                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25331         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25332         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25333         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25334     }
25335 
25336     @Test
25337     public void testConstructor198() throws WrongSizeException {
25338         final Collection<StandardDeviationBodyKinematics> measurements =
25339                 Collections.emptyList();
25340 
25341         final Matrix ba = generateBa();
25342         final double biasX = ba.getElementAtIndex(0);
25343         final double biasY = ba.getElementAtIndex(1);
25344         final double biasZ = ba.getElementAtIndex(2);
25345 
25346         final Matrix ma = generateMaCommonAxis();
25347         final double sx = ma.getElementAt(0, 0);
25348         final double sy = ma.getElementAt(1, 1);
25349         final double sz = ma.getElementAt(2, 2);
25350         final double mxy = ma.getElementAt(0, 1);
25351         final double mxz = ma.getElementAt(0, 2);
25352         final double myx = ma.getElementAt(1, 0);
25353         final double myz = ma.getElementAt(1, 2);
25354         final double mzx = ma.getElementAt(2, 0);
25355         final double mzy = ma.getElementAt(2, 1);
25356 
25357         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25358         final double latitude = Math.toRadians(
25359                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25360         final double longitude = Math.toRadians(
25361                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25362         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25363         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25364         final NEDVelocity nedVelocity = new NEDVelocity();
25365         final ECEFPosition ecefPosition = new ECEFPosition();
25366         final ECEFVelocity ecefVelocity = new ECEFVelocity();
25367         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25368                 ecefPosition, ecefVelocity);
25369 
25370         final KnownPositionAccelerometerCalibrator calibrator =
25371                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
25372                         biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
25373                         myx, myz, mzx, mzy, this);
25374 
25375         // check default values
25376         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25377         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25378         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25379         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25380         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25381         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25382         final Acceleration bx2 = new Acceleration(0.0,
25383                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25384         calibrator.getInitialBiasXAsAcceleration(bx2);
25385         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25386         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25387         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25388         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25389         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25390         final Acceleration by2 = new Acceleration(0.0,
25391                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25392         calibrator.getInitialBiasYAsAcceleration(by2);
25393         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25394         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25395         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25396         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25397         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25398         final Acceleration bz2 = new Acceleration(0.0,
25399                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25400         calibrator.getInitialBiasZAsAcceleration(bz2);
25401         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25402         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25403         assertEquals(calibrator.getInitialSx(), sx, 0.0);
25404         assertEquals(calibrator.getInitialSy(), sy, 0.0);
25405         assertEquals(calibrator.getInitialSz(), sz, 0.0);
25406         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25407         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25408         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25409         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25410         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25411         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25412         final double[] bias1 = calibrator.getInitialBias();
25413         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25414         final double[] bias2 = new double[3];
25415         calibrator.getInitialBias(bias2);
25416         assertArrayEquals(bias1, bias2, 0.0);
25417         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25418         assertEquals(b1, ba);
25419         final Matrix b2 = new Matrix(3, 1);
25420         calibrator.getInitialBiasAsMatrix(b2);
25421         assertEquals(b1, b2);
25422         final Matrix ma1 = new Matrix(3, 3);
25423         ma1.setSubmatrix(0, 0,
25424                 2, 2,
25425                 new double[]{sx, myx, mzx,
25426                         mxy, sy, mzy,
25427                         mxz, myz, sz});
25428         assertEquals(calibrator.getInitialMa(), ma1);
25429         final Matrix ma2 = new Matrix(3, 3);
25430         calibrator.getInitialMa(ma2);
25431         assertEquals(ma1, ma2);
25432         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25433         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25434         final NEDPosition nedPosition1 = new NEDPosition();
25435         assertTrue(calibrator.getNedPosition(nedPosition1));
25436         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25437         assertSame(calibrator.getMeasurements(), measurements);
25438         assertFalse(calibrator.isCommonAxisUsed());
25439         assertSame(calibrator.getListener(), this);
25440         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
25441         assertFalse(calibrator.isReady());
25442         assertFalse(calibrator.isRunning());
25443         assertNull(calibrator.getEstimatedBiases());
25444         assertFalse(calibrator.getEstimatedBiases(null));
25445         assertNull(calibrator.getEstimatedBiasesAsMatrix());
25446         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25447         assertNull(calibrator.getEstimatedBiasFx());
25448         assertNull(calibrator.getEstimatedBiasFy());
25449         assertNull(calibrator.getEstimatedBiasFz());
25450         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25451         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25452         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25453         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25454         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25455         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25456         assertNull(calibrator.getEstimatedMa());
25457         assertNull(calibrator.getEstimatedSx());
25458         assertNull(calibrator.getEstimatedSy());
25459         assertNull(calibrator.getEstimatedSz());
25460         assertNull(calibrator.getEstimatedMxy());
25461         assertNull(calibrator.getEstimatedMxz());
25462         assertNull(calibrator.getEstimatedMyx());
25463         assertNull(calibrator.getEstimatedMyz());
25464         assertNull(calibrator.getEstimatedMzx());
25465         assertNull(calibrator.getEstimatedMzy());
25466         assertNull(calibrator.getEstimatedCovariance());
25467         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25468         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25469                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25470         assertNotNull(calibrator.getGroundTruthGravityNorm());
25471         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25472         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25473         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25474                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25475         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25476         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25477         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25478     }
25479 
25480     @Test
25481     public void testConstructor199() throws WrongSizeException {
25482         final Matrix ba = generateBa();
25483         final double biasX = ba.getElementAtIndex(0);
25484         final double biasY = ba.getElementAtIndex(1);
25485         final double biasZ = ba.getElementAtIndex(2);
25486 
25487         final Matrix ma = generateMaCommonAxis();
25488         final double sx = ma.getElementAt(0, 0);
25489         final double sy = ma.getElementAt(1, 1);
25490         final double sz = ma.getElementAt(2, 2);
25491         final double mxy = ma.getElementAt(0, 1);
25492         final double mxz = ma.getElementAt(0, 2);
25493         final double myx = ma.getElementAt(1, 0);
25494         final double myz = ma.getElementAt(1, 2);
25495         final double mzx = ma.getElementAt(2, 0);
25496         final double mzy = ma.getElementAt(2, 1);
25497 
25498         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25499         final double latitude = Math.toRadians(
25500                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25501         final double longitude = Math.toRadians(
25502                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25503         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25504         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25505         final NEDVelocity nedVelocity = new NEDVelocity();
25506         final ECEFPosition ecefPosition = new ECEFPosition();
25507         final ECEFVelocity ecefVelocity = new ECEFVelocity();
25508         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25509                 ecefPosition, ecefVelocity);
25510 
25511         final KnownPositionAccelerometerCalibrator calibrator =
25512                 new KnownPositionAccelerometerCalibrator(nedPosition,
25513                         true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
25514                         myx, myz, mzx, mzy);
25515 
25516         // check default values
25517         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25518         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25519         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25520         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25521         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25522         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25523         final Acceleration bx2 = new Acceleration(0.0,
25524                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25525         calibrator.getInitialBiasXAsAcceleration(bx2);
25526         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25527         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25528         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25529         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25530         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25531         final Acceleration by2 = new Acceleration(0.0,
25532                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25533         calibrator.getInitialBiasYAsAcceleration(by2);
25534         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25535         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25536         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25537         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25538         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25539         final Acceleration bz2 = new Acceleration(0.0,
25540                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25541         calibrator.getInitialBiasZAsAcceleration(bz2);
25542         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25543         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25544         assertEquals(calibrator.getInitialSx(), sx, 0.0);
25545         assertEquals(calibrator.getInitialSy(), sy, 0.0);
25546         assertEquals(calibrator.getInitialSz(), sz, 0.0);
25547         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25548         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25549         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25550         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25551         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25552         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25553         final double[] bias1 = calibrator.getInitialBias();
25554         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25555         final double[] bias2 = new double[3];
25556         calibrator.getInitialBias(bias2);
25557         assertArrayEquals(bias1, bias2, 0.0);
25558         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25559         assertEquals(b1, ba);
25560         final Matrix b2 = new Matrix(3, 1);
25561         calibrator.getInitialBiasAsMatrix(b2);
25562         assertEquals(b1, b2);
25563         final Matrix ma1 = new Matrix(3, 3);
25564         ma1.setSubmatrix(0, 0,
25565                 2, 2,
25566                 new double[]{sx, myx, mzx,
25567                         mxy, sy, mzy,
25568                         mxz, myz, sz});
25569         assertEquals(calibrator.getInitialMa(), ma1);
25570         final Matrix ma2 = new Matrix(3, 3);
25571         calibrator.getInitialMa(ma2);
25572         assertEquals(ma1, ma2);
25573         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25574         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25575         final NEDPosition nedPosition1 = new NEDPosition();
25576         assertTrue(calibrator.getNedPosition(nedPosition1));
25577         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25578         assertNull(calibrator.getMeasurements());
25579         assertTrue(calibrator.isCommonAxisUsed());
25580         assertNull(calibrator.getListener());
25581         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
25582         assertFalse(calibrator.isReady());
25583         assertFalse(calibrator.isRunning());
25584         assertNull(calibrator.getEstimatedBiases());
25585         assertFalse(calibrator.getEstimatedBiases(null));
25586         assertNull(calibrator.getEstimatedBiasesAsMatrix());
25587         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25588         assertNull(calibrator.getEstimatedBiasFx());
25589         assertNull(calibrator.getEstimatedBiasFy());
25590         assertNull(calibrator.getEstimatedBiasFz());
25591         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25592         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25593         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25594         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25595         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25596         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25597         assertNull(calibrator.getEstimatedMa());
25598         assertNull(calibrator.getEstimatedSx());
25599         assertNull(calibrator.getEstimatedSy());
25600         assertNull(calibrator.getEstimatedSz());
25601         assertNull(calibrator.getEstimatedMxy());
25602         assertNull(calibrator.getEstimatedMxz());
25603         assertNull(calibrator.getEstimatedMyx());
25604         assertNull(calibrator.getEstimatedMyz());
25605         assertNull(calibrator.getEstimatedMzx());
25606         assertNull(calibrator.getEstimatedMzy());
25607         assertNull(calibrator.getEstimatedCovariance());
25608         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25609         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25610                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25611         assertNotNull(calibrator.getGroundTruthGravityNorm());
25612         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25613         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25614         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25615                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25616         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25617         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25618         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25619     }
25620 
25621     @Test
25622     public void testConstructor200() throws WrongSizeException {
25623         final Matrix ba = generateBa();
25624         final double biasX = ba.getElementAtIndex(0);
25625         final double biasY = ba.getElementAtIndex(1);
25626         final double biasZ = ba.getElementAtIndex(2);
25627 
25628         final Matrix ma = generateMaCommonAxis();
25629         final double sx = ma.getElementAt(0, 0);
25630         final double sy = ma.getElementAt(1, 1);
25631         final double sz = ma.getElementAt(2, 2);
25632         final double mxy = ma.getElementAt(0, 1);
25633         final double mxz = ma.getElementAt(0, 2);
25634         final double myx = ma.getElementAt(1, 0);
25635         final double myz = ma.getElementAt(1, 2);
25636         final double mzx = ma.getElementAt(2, 0);
25637         final double mzy = ma.getElementAt(2, 1);
25638 
25639         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25640         final double latitude = Math.toRadians(
25641                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25642         final double longitude = Math.toRadians(
25643                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25644         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25645         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25646         final NEDVelocity nedVelocity = new NEDVelocity();
25647         final ECEFPosition ecefPosition = new ECEFPosition();
25648         final ECEFVelocity ecefVelocity = new ECEFVelocity();
25649         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25650                 ecefPosition, ecefVelocity);
25651 
25652         final KnownPositionAccelerometerCalibrator calibrator =
25653                 new KnownPositionAccelerometerCalibrator(nedPosition,
25654                         true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
25655                         myx, myz, mzx, mzy, this);
25656 
25657         // check default values
25658         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25659         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25660         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25661         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25662         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25663         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25664         final Acceleration bx2 = new Acceleration(0.0,
25665                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25666         calibrator.getInitialBiasXAsAcceleration(bx2);
25667         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25668         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25669         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25670         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25671         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25672         final Acceleration by2 = new Acceleration(0.0,
25673                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25674         calibrator.getInitialBiasYAsAcceleration(by2);
25675         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25676         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25677         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25678         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25679         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25680         final Acceleration bz2 = new Acceleration(0.0,
25681                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25682         calibrator.getInitialBiasZAsAcceleration(bz2);
25683         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25684         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25685         assertEquals(calibrator.getInitialSx(), sx, 0.0);
25686         assertEquals(calibrator.getInitialSy(), sy, 0.0);
25687         assertEquals(calibrator.getInitialSz(), sz, 0.0);
25688         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25689         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25690         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25691         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25692         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25693         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25694         final double[] bias1 = calibrator.getInitialBias();
25695         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25696         final double[] bias2 = new double[3];
25697         calibrator.getInitialBias(bias2);
25698         assertArrayEquals(bias1, bias2, 0.0);
25699         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25700         assertEquals(b1, ba);
25701         final Matrix b2 = new Matrix(3, 1);
25702         calibrator.getInitialBiasAsMatrix(b2);
25703         assertEquals(b1, b2);
25704         final Matrix ma1 = new Matrix(3, 3);
25705         ma1.setSubmatrix(0, 0,
25706                 2, 2,
25707                 new double[]{sx, myx, mzx,
25708                         mxy, sy, mzy,
25709                         mxz, myz, sz});
25710         assertEquals(calibrator.getInitialMa(), ma1);
25711         final Matrix ma2 = new Matrix(3, 3);
25712         calibrator.getInitialMa(ma2);
25713         assertEquals(ma1, ma2);
25714         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25715         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25716         final NEDPosition nedPosition1 = new NEDPosition();
25717         assertTrue(calibrator.getNedPosition(nedPosition1));
25718         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25719         assertNull(calibrator.getMeasurements());
25720         assertTrue(calibrator.isCommonAxisUsed());
25721         assertSame(calibrator.getListener(), this);
25722         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
25723         assertFalse(calibrator.isReady());
25724         assertFalse(calibrator.isRunning());
25725         assertNull(calibrator.getEstimatedBiases());
25726         assertFalse(calibrator.getEstimatedBiases(null));
25727         assertNull(calibrator.getEstimatedBiasesAsMatrix());
25728         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25729         assertNull(calibrator.getEstimatedBiasFx());
25730         assertNull(calibrator.getEstimatedBiasFy());
25731         assertNull(calibrator.getEstimatedBiasFz());
25732         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25733         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25734         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25735         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25736         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25737         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25738         assertNull(calibrator.getEstimatedMa());
25739         assertNull(calibrator.getEstimatedSx());
25740         assertNull(calibrator.getEstimatedSy());
25741         assertNull(calibrator.getEstimatedSz());
25742         assertNull(calibrator.getEstimatedMxy());
25743         assertNull(calibrator.getEstimatedMxz());
25744         assertNull(calibrator.getEstimatedMyx());
25745         assertNull(calibrator.getEstimatedMyz());
25746         assertNull(calibrator.getEstimatedMzx());
25747         assertNull(calibrator.getEstimatedMzy());
25748         assertNull(calibrator.getEstimatedCovariance());
25749         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25750         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25751                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25752         assertNotNull(calibrator.getGroundTruthGravityNorm());
25753         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25754         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25755         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25756                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25757         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25758         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25759         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25760     }
25761 
25762     @Test
25763     public void testConstructor201() throws WrongSizeException {
25764         final Collection<StandardDeviationBodyKinematics> measurements =
25765                 Collections.emptyList();
25766 
25767         final Matrix ba = generateBa();
25768         final double biasX = ba.getElementAtIndex(0);
25769         final double biasY = ba.getElementAtIndex(1);
25770         final double biasZ = ba.getElementAtIndex(2);
25771 
25772         final Matrix ma = generateMaCommonAxis();
25773         final double sx = ma.getElementAt(0, 0);
25774         final double sy = ma.getElementAt(1, 1);
25775         final double sz = ma.getElementAt(2, 2);
25776         final double mxy = ma.getElementAt(0, 1);
25777         final double mxz = ma.getElementAt(0, 2);
25778         final double myx = ma.getElementAt(1, 0);
25779         final double myz = ma.getElementAt(1, 2);
25780         final double mzx = ma.getElementAt(2, 0);
25781         final double mzy = ma.getElementAt(2, 1);
25782 
25783         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25784         final double latitude = Math.toRadians(
25785                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25786         final double longitude = Math.toRadians(
25787                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25788         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25789         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25790         final NEDVelocity nedVelocity = new NEDVelocity();
25791         final ECEFPosition ecefPosition = new ECEFPosition();
25792         final ECEFVelocity ecefVelocity = new ECEFVelocity();
25793         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25794                 ecefPosition, ecefVelocity);
25795 
25796         final KnownPositionAccelerometerCalibrator calibrator =
25797                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
25798                         true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
25799                         myx, myz, mzx, mzy);
25800 
25801         // check default values
25802         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25803         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25804         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25805         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25806         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25807         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25808         final Acceleration bx2 = new Acceleration(0.0,
25809                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25810         calibrator.getInitialBiasXAsAcceleration(bx2);
25811         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25812         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25813         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25814         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25815         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25816         final Acceleration by2 = new Acceleration(0.0,
25817                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25818         calibrator.getInitialBiasYAsAcceleration(by2);
25819         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25820         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25821         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25822         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25823         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25824         final Acceleration bz2 = new Acceleration(0.0,
25825                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25826         calibrator.getInitialBiasZAsAcceleration(bz2);
25827         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25828         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25829         assertEquals(calibrator.getInitialSx(), sx, 0.0);
25830         assertEquals(calibrator.getInitialSy(), sy, 0.0);
25831         assertEquals(calibrator.getInitialSz(), sz, 0.0);
25832         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25833         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25834         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25835         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25836         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25837         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25838         final double[] bias1 = calibrator.getInitialBias();
25839         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25840         final double[] bias2 = new double[3];
25841         calibrator.getInitialBias(bias2);
25842         assertArrayEquals(bias1, bias2, 0.0);
25843         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25844         assertEquals(b1, ba);
25845         final Matrix b2 = new Matrix(3, 1);
25846         calibrator.getInitialBiasAsMatrix(b2);
25847         assertEquals(b1, b2);
25848         final Matrix ma1 = new Matrix(3, 3);
25849         ma1.setSubmatrix(0, 0,
25850                 2, 2,
25851                 new double[]{sx, myx, mzx,
25852                         mxy, sy, mzy,
25853                         mxz, myz, sz});
25854         assertEquals(calibrator.getInitialMa(), ma1);
25855         final Matrix ma2 = new Matrix(3, 3);
25856         calibrator.getInitialMa(ma2);
25857         assertEquals(ma1, ma2);
25858         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25859         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25860         final NEDPosition nedPosition1 = new NEDPosition();
25861         assertTrue(calibrator.getNedPosition(nedPosition1));
25862         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25863         assertSame(calibrator.getMeasurements(), measurements);
25864         assertTrue(calibrator.isCommonAxisUsed());
25865         assertNull(calibrator.getListener());
25866         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
25867         assertFalse(calibrator.isReady());
25868         assertFalse(calibrator.isRunning());
25869         assertNull(calibrator.getEstimatedBiases());
25870         assertFalse(calibrator.getEstimatedBiases(null));
25871         assertNull(calibrator.getEstimatedBiasesAsMatrix());
25872         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25873         assertNull(calibrator.getEstimatedBiasFx());
25874         assertNull(calibrator.getEstimatedBiasFy());
25875         assertNull(calibrator.getEstimatedBiasFz());
25876         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25877         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25878         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25879         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25880         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25881         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25882         assertNull(calibrator.getEstimatedMa());
25883         assertNull(calibrator.getEstimatedSx());
25884         assertNull(calibrator.getEstimatedSy());
25885         assertNull(calibrator.getEstimatedSz());
25886         assertNull(calibrator.getEstimatedMxy());
25887         assertNull(calibrator.getEstimatedMxz());
25888         assertNull(calibrator.getEstimatedMyx());
25889         assertNull(calibrator.getEstimatedMyz());
25890         assertNull(calibrator.getEstimatedMzx());
25891         assertNull(calibrator.getEstimatedMzy());
25892         assertNull(calibrator.getEstimatedCovariance());
25893         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25894         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25895                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25896         assertNotNull(calibrator.getGroundTruthGravityNorm());
25897         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25898         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25899         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25900                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25901         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25902         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25903         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25904     }
25905 
25906     @Test
25907     public void testConstructor202() throws WrongSizeException {
25908         final Collection<StandardDeviationBodyKinematics> measurements =
25909                 Collections.emptyList();
25910 
25911         final Matrix ba = generateBa();
25912         final double biasX = ba.getElementAtIndex(0);
25913         final double biasY = ba.getElementAtIndex(1);
25914         final double biasZ = ba.getElementAtIndex(2);
25915 
25916         final Matrix ma = generateMaCommonAxis();
25917         final double sx = ma.getElementAt(0, 0);
25918         final double sy = ma.getElementAt(1, 1);
25919         final double sz = ma.getElementAt(2, 2);
25920         final double mxy = ma.getElementAt(0, 1);
25921         final double mxz = ma.getElementAt(0, 2);
25922         final double myx = ma.getElementAt(1, 0);
25923         final double myz = ma.getElementAt(1, 2);
25924         final double mzx = ma.getElementAt(2, 0);
25925         final double mzy = ma.getElementAt(2, 1);
25926 
25927         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25928         final double latitude = Math.toRadians(
25929                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25930         final double longitude = Math.toRadians(
25931                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25932         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25933         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25934         final NEDVelocity nedVelocity = new NEDVelocity();
25935         final ECEFPosition ecefPosition = new ECEFPosition();
25936         final ECEFVelocity ecefVelocity = new ECEFVelocity();
25937         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25938                 ecefPosition, ecefVelocity);
25939 
25940         final KnownPositionAccelerometerCalibrator calibrator =
25941                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
25942                         true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
25943                         myx, myz, mzx, mzy, this);
25944 
25945         // check default values
25946         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25947         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25948         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25949         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25950         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25951         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25952         final Acceleration bx2 = new Acceleration(0.0,
25953                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25954         calibrator.getInitialBiasXAsAcceleration(bx2);
25955         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25956         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25957         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25958         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25959         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25960         final Acceleration by2 = new Acceleration(0.0,
25961                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25962         calibrator.getInitialBiasYAsAcceleration(by2);
25963         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25964         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25965         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25966         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25967         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25968         final Acceleration bz2 = new Acceleration(0.0,
25969                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25970         calibrator.getInitialBiasZAsAcceleration(bz2);
25971         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25972         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25973         assertEquals(calibrator.getInitialSx(), sx, 0.0);
25974         assertEquals(calibrator.getInitialSy(), sy, 0.0);
25975         assertEquals(calibrator.getInitialSz(), sz, 0.0);
25976         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25977         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25978         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25979         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25980         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25981         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25982         final double[] bias1 = calibrator.getInitialBias();
25983         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25984         final double[] bias2 = new double[3];
25985         calibrator.getInitialBias(bias2);
25986         assertArrayEquals(bias1, bias2, 0.0);
25987         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25988         assertEquals(b1, ba);
25989         final Matrix b2 = new Matrix(3, 1);
25990         calibrator.getInitialBiasAsMatrix(b2);
25991         assertEquals(b1, b2);
25992         final Matrix ma1 = new Matrix(3, 3);
25993         ma1.setSubmatrix(0, 0,
25994                 2, 2,
25995                 new double[]{sx, myx, mzx,
25996                         mxy, sy, mzy,
25997                         mxz, myz, sz});
25998         assertEquals(calibrator.getInitialMa(), ma1);
25999         final Matrix ma2 = new Matrix(3, 3);
26000         calibrator.getInitialMa(ma2);
26001         assertEquals(ma1, ma2);
26002         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26003         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26004         final NEDPosition nedPosition1 = new NEDPosition();
26005         assertTrue(calibrator.getNedPosition(nedPosition1));
26006         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26007         assertSame(calibrator.getMeasurements(), measurements);
26008         assertTrue(calibrator.isCommonAxisUsed());
26009         assertSame(calibrator.getListener(), this);
26010         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
26011         assertFalse(calibrator.isReady());
26012         assertFalse(calibrator.isRunning());
26013         assertNull(calibrator.getEstimatedBiases());
26014         assertFalse(calibrator.getEstimatedBiases(null));
26015         assertNull(calibrator.getEstimatedBiasesAsMatrix());
26016         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26017         assertNull(calibrator.getEstimatedBiasFx());
26018         assertNull(calibrator.getEstimatedBiasFy());
26019         assertNull(calibrator.getEstimatedBiasFz());
26020         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26021         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26022         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26023         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26024         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26025         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26026         assertNull(calibrator.getEstimatedMa());
26027         assertNull(calibrator.getEstimatedSx());
26028         assertNull(calibrator.getEstimatedSy());
26029         assertNull(calibrator.getEstimatedSz());
26030         assertNull(calibrator.getEstimatedMxy());
26031         assertNull(calibrator.getEstimatedMxz());
26032         assertNull(calibrator.getEstimatedMyx());
26033         assertNull(calibrator.getEstimatedMyz());
26034         assertNull(calibrator.getEstimatedMzx());
26035         assertNull(calibrator.getEstimatedMzy());
26036         assertNull(calibrator.getEstimatedCovariance());
26037         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26038         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26039                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26040         assertNotNull(calibrator.getGroundTruthGravityNorm());
26041         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26042         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26043         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26044                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26045         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26046         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26047         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26048     }
26049 
26050     @Test
26051     public void testConstructor203() throws WrongSizeException {
26052         final Matrix ba = generateBa();
26053         final double biasX = ba.getElementAtIndex(0);
26054         final double biasY = ba.getElementAtIndex(1);
26055         final double biasZ = ba.getElementAtIndex(2);
26056 
26057         final Acceleration bx = new Acceleration(biasX,
26058                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26059         final Acceleration by = new Acceleration(biasY,
26060                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26061         final Acceleration bz = new Acceleration(biasZ,
26062                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26063 
26064         final Matrix ma = generateMaCommonAxis();
26065         final double sx = ma.getElementAt(0, 0);
26066         final double sy = ma.getElementAt(1, 1);
26067         final double sz = ma.getElementAt(2, 2);
26068         final double mxy = ma.getElementAt(0, 1);
26069         final double mxz = ma.getElementAt(0, 2);
26070         final double myx = ma.getElementAt(1, 0);
26071         final double myz = ma.getElementAt(1, 2);
26072         final double mzx = ma.getElementAt(2, 0);
26073         final double mzy = ma.getElementAt(2, 1);
26074 
26075         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26076         final double latitude = Math.toRadians(
26077                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26078         final double longitude = Math.toRadians(
26079                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26080         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26081         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26082         final NEDVelocity nedVelocity = new NEDVelocity();
26083         final ECEFPosition ecefPosition = new ECEFPosition();
26084         final ECEFVelocity ecefVelocity = new ECEFVelocity();
26085         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26086                 ecefPosition, ecefVelocity);
26087 
26088         final KnownPositionAccelerometerCalibrator calibrator =
26089                 new KnownPositionAccelerometerCalibrator(nedPosition,
26090                         bx, by, bz, sx, sy, sz, mxy, mxz,
26091                         myx, myz, mzx, mzy);
26092 
26093         // check default values
26094         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26095         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26096         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26097         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26098         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26099         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26100         final Acceleration bx2 = new Acceleration(0.0,
26101                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26102         calibrator.getInitialBiasXAsAcceleration(bx2);
26103         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26104         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26105         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26106         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26107         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26108         final Acceleration by2 = new Acceleration(0.0,
26109                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26110         calibrator.getInitialBiasYAsAcceleration(by2);
26111         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26112         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26113         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26114         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26115         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26116         final Acceleration bz2 = new Acceleration(0.0,
26117                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26118         calibrator.getInitialBiasZAsAcceleration(bz2);
26119         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26120         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26121         assertEquals(calibrator.getInitialSx(), sx, 0.0);
26122         assertEquals(calibrator.getInitialSy(), sy, 0.0);
26123         assertEquals(calibrator.getInitialSz(), sz, 0.0);
26124         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
26125         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
26126         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
26127         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
26128         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
26129         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
26130         final double[] bias1 = calibrator.getInitialBias();
26131         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
26132         final double[] bias2 = new double[3];
26133         calibrator.getInitialBias(bias2);
26134         assertArrayEquals(bias1, bias2, 0.0);
26135         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26136         assertEquals(b1, ba);
26137         final Matrix b2 = new Matrix(3, 1);
26138         calibrator.getInitialBiasAsMatrix(b2);
26139         assertEquals(b1, b2);
26140         final Matrix ma1 = new Matrix(3, 3);
26141         ma1.setSubmatrix(0, 0,
26142                 2, 2,
26143                 new double[]{sx, myx, mzx,
26144                         mxy, sy, mzy,
26145                         mxz, myz, sz});
26146         assertEquals(calibrator.getInitialMa(), ma1);
26147         final Matrix ma2 = new Matrix(3, 3);
26148         calibrator.getInitialMa(ma2);
26149         assertEquals(ma1, ma2);
26150         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26151         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26152         final NEDPosition nedPosition1 = new NEDPosition();
26153         assertTrue(calibrator.getNedPosition(nedPosition1));
26154         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26155         assertNull(calibrator.getMeasurements());
26156         assertFalse(calibrator.isCommonAxisUsed());
26157         assertNull(calibrator.getListener());
26158         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
26159         assertFalse(calibrator.isReady());
26160         assertFalse(calibrator.isRunning());
26161         assertNull(calibrator.getEstimatedBiases());
26162         assertFalse(calibrator.getEstimatedBiases(null));
26163         assertNull(calibrator.getEstimatedBiasesAsMatrix());
26164         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26165         assertNull(calibrator.getEstimatedBiasFx());
26166         assertNull(calibrator.getEstimatedBiasFy());
26167         assertNull(calibrator.getEstimatedBiasFz());
26168         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26169         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26170         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26171         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26172         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26173         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26174         assertNull(calibrator.getEstimatedMa());
26175         assertNull(calibrator.getEstimatedSx());
26176         assertNull(calibrator.getEstimatedSy());
26177         assertNull(calibrator.getEstimatedSz());
26178         assertNull(calibrator.getEstimatedMxy());
26179         assertNull(calibrator.getEstimatedMxz());
26180         assertNull(calibrator.getEstimatedMyx());
26181         assertNull(calibrator.getEstimatedMyz());
26182         assertNull(calibrator.getEstimatedMzx());
26183         assertNull(calibrator.getEstimatedMzy());
26184         assertNull(calibrator.getEstimatedCovariance());
26185         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26186         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26187                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26188         assertNotNull(calibrator.getGroundTruthGravityNorm());
26189         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26190         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26191         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26192                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26193         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26194         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26195         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26196     }
26197 
26198     @Test
26199     public void testConstructor204() throws WrongSizeException {
26200         final Matrix ba = generateBa();
26201         final double biasX = ba.getElementAtIndex(0);
26202         final double biasY = ba.getElementAtIndex(1);
26203         final double biasZ = ba.getElementAtIndex(2);
26204 
26205         final Acceleration bx = new Acceleration(biasX,
26206                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26207         final Acceleration by = new Acceleration(biasY,
26208                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26209         final Acceleration bz = new Acceleration(biasZ,
26210                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26211 
26212         final Matrix ma = generateMaCommonAxis();
26213         final double sx = ma.getElementAt(0, 0);
26214         final double sy = ma.getElementAt(1, 1);
26215         final double sz = ma.getElementAt(2, 2);
26216         final double mxy = ma.getElementAt(0, 1);
26217         final double mxz = ma.getElementAt(0, 2);
26218         final double myx = ma.getElementAt(1, 0);
26219         final double myz = ma.getElementAt(1, 2);
26220         final double mzx = ma.getElementAt(2, 0);
26221         final double mzy = ma.getElementAt(2, 1);
26222 
26223         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26224         final double latitude = Math.toRadians(
26225                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26226         final double longitude = Math.toRadians(
26227                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26228         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26229         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26230         final NEDVelocity nedVelocity = new NEDVelocity();
26231         final ECEFPosition ecefPosition = new ECEFPosition();
26232         final ECEFVelocity ecefVelocity = new ECEFVelocity();
26233         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26234                 ecefPosition, ecefVelocity);
26235 
26236         final KnownPositionAccelerometerCalibrator calibrator =
26237                 new KnownPositionAccelerometerCalibrator(nedPosition,
26238                         bx, by, bz, sx, sy, sz, mxy, mxz,
26239                         myx, myz, mzx, mzy, this);
26240 
26241         // check default values
26242         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26243         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26244         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26245         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26246         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26247         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26248         final Acceleration bx2 = new Acceleration(0.0,
26249                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26250         calibrator.getInitialBiasXAsAcceleration(bx2);
26251         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26252         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26253         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26254         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26255         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26256         final Acceleration by2 = new Acceleration(0.0,
26257                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26258         calibrator.getInitialBiasYAsAcceleration(by2);
26259         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26260         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26261         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26262         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26263         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26264         final Acceleration bz2 = new Acceleration(0.0,
26265                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26266         calibrator.getInitialBiasZAsAcceleration(bz2);
26267         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26268         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26269         assertEquals(calibrator.getInitialSx(), sx, 0.0);
26270         assertEquals(calibrator.getInitialSy(), sy, 0.0);
26271         assertEquals(calibrator.getInitialSz(), sz, 0.0);
26272         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
26273         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
26274         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
26275         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
26276         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
26277         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
26278         final double[] bias1 = calibrator.getInitialBias();
26279         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
26280         final double[] bias2 = new double[3];
26281         calibrator.getInitialBias(bias2);
26282         assertArrayEquals(bias1, bias2, 0.0);
26283         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26284         assertEquals(b1, ba);
26285         final Matrix b2 = new Matrix(3, 1);
26286         calibrator.getInitialBiasAsMatrix(b2);
26287         assertEquals(b1, b2);
26288         final Matrix ma1 = new Matrix(3, 3);
26289         ma1.setSubmatrix(0, 0,
26290                 2, 2,
26291                 new double[]{sx, myx, mzx,
26292                         mxy, sy, mzy,
26293                         mxz, myz, sz});
26294         assertEquals(calibrator.getInitialMa(), ma1);
26295         final Matrix ma2 = new Matrix(3, 3);
26296         calibrator.getInitialMa(ma2);
26297         assertEquals(ma1, ma2);
26298         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26299         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26300         final NEDPosition nedPosition1 = new NEDPosition();
26301         assertTrue(calibrator.getNedPosition(nedPosition1));
26302         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26303         assertNull(calibrator.getMeasurements());
26304         assertFalse(calibrator.isCommonAxisUsed());
26305         assertSame(calibrator.getListener(), this);
26306         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
26307         assertFalse(calibrator.isReady());
26308         assertFalse(calibrator.isRunning());
26309         assertNull(calibrator.getEstimatedBiases());
26310         assertFalse(calibrator.getEstimatedBiases(null));
26311         assertNull(calibrator.getEstimatedBiasesAsMatrix());
26312         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26313         assertNull(calibrator.getEstimatedBiasFx());
26314         assertNull(calibrator.getEstimatedBiasFy());
26315         assertNull(calibrator.getEstimatedBiasFz());
26316         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26317         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26318         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26319         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26320         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26321         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26322         assertNull(calibrator.getEstimatedMa());
26323         assertNull(calibrator.getEstimatedSx());
26324         assertNull(calibrator.getEstimatedSy());
26325         assertNull(calibrator.getEstimatedSz());
26326         assertNull(calibrator.getEstimatedMxy());
26327         assertNull(calibrator.getEstimatedMxz());
26328         assertNull(calibrator.getEstimatedMyx());
26329         assertNull(calibrator.getEstimatedMyz());
26330         assertNull(calibrator.getEstimatedMzx());
26331         assertNull(calibrator.getEstimatedMzy());
26332         assertNull(calibrator.getEstimatedCovariance());
26333         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26334         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26335                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26336         assertNotNull(calibrator.getGroundTruthGravityNorm());
26337         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26338         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26339         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26340                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26341         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26342         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26343         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26344     }
26345 
26346     @Test
26347     public void testConstructor205() throws WrongSizeException {
26348         final Collection<StandardDeviationBodyKinematics> measurements =
26349                 Collections.emptyList();
26350 
26351         final Matrix ba = generateBa();
26352         final double biasX = ba.getElementAtIndex(0);
26353         final double biasY = ba.getElementAtIndex(1);
26354         final double biasZ = ba.getElementAtIndex(2);
26355 
26356         final Acceleration bx = new Acceleration(biasX,
26357                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26358         final Acceleration by = new Acceleration(biasY,
26359                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26360         final Acceleration bz = new Acceleration(biasZ,
26361                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26362 
26363         final Matrix ma = generateMaCommonAxis();
26364         final double sx = ma.getElementAt(0, 0);
26365         final double sy = ma.getElementAt(1, 1);
26366         final double sz = ma.getElementAt(2, 2);
26367         final double mxy = ma.getElementAt(0, 1);
26368         final double mxz = ma.getElementAt(0, 2);
26369         final double myx = ma.getElementAt(1, 0);
26370         final double myz = ma.getElementAt(1, 2);
26371         final double mzx = ma.getElementAt(2, 0);
26372         final double mzy = ma.getElementAt(2, 1);
26373 
26374         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26375         final double latitude = Math.toRadians(
26376                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26377         final double longitude = Math.toRadians(
26378                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26379         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26380         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26381         final NEDVelocity nedVelocity = new NEDVelocity();
26382         final ECEFPosition ecefPosition = new ECEFPosition();
26383         final ECEFVelocity ecefVelocity = new ECEFVelocity();
26384         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26385                 ecefPosition, ecefVelocity);
26386 
26387         final KnownPositionAccelerometerCalibrator calibrator =
26388                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
26389                         bx, by, bz, sx, sy, sz, mxy, mxz,
26390                         myx, myz, mzx, mzy);
26391 
26392         // check default values
26393         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26394         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26395         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26396         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26397         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26398         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26399         final Acceleration bx2 = new Acceleration(0.0,
26400                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26401         calibrator.getInitialBiasXAsAcceleration(bx2);
26402         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26403         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26404         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26405         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26406         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26407         final Acceleration by2 = new Acceleration(0.0,
26408                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26409         calibrator.getInitialBiasYAsAcceleration(by2);
26410         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26411         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26412         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26413         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26414         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26415         final Acceleration bz2 = new Acceleration(0.0,
26416                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26417         calibrator.getInitialBiasZAsAcceleration(bz2);
26418         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26419         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26420         assertEquals(calibrator.getInitialSx(), sx, 0.0);
26421         assertEquals(calibrator.getInitialSy(), sy, 0.0);
26422         assertEquals(calibrator.getInitialSz(), sz, 0.0);
26423         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
26424         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
26425         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
26426         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
26427         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
26428         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
26429         final double[] bias1 = calibrator.getInitialBias();
26430         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
26431         final double[] bias2 = new double[3];
26432         calibrator.getInitialBias(bias2);
26433         assertArrayEquals(bias1, bias2, 0.0);
26434         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26435         assertEquals(b1, ba);
26436         final Matrix b2 = new Matrix(3, 1);
26437         calibrator.getInitialBiasAsMatrix(b2);
26438         assertEquals(b1, b2);
26439         final Matrix ma1 = new Matrix(3, 3);
26440         ma1.setSubmatrix(0, 0,
26441                 2, 2,
26442                 new double[]{sx, myx, mzx,
26443                         mxy, sy, mzy,
26444                         mxz, myz, sz});
26445         assertEquals(calibrator.getInitialMa(), ma1);
26446         final Matrix ma2 = new Matrix(3, 3);
26447         calibrator.getInitialMa(ma2);
26448         assertEquals(ma1, ma2);
26449         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26450         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26451         final NEDPosition nedPosition1 = new NEDPosition();
26452         assertTrue(calibrator.getNedPosition(nedPosition1));
26453         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26454         assertSame(calibrator.getMeasurements(), measurements);
26455         assertFalse(calibrator.isCommonAxisUsed());
26456         assertNull(calibrator.getListener());
26457         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
26458         assertFalse(calibrator.isReady());
26459         assertFalse(calibrator.isRunning());
26460         assertNull(calibrator.getEstimatedBiases());
26461         assertFalse(calibrator.getEstimatedBiases(null));
26462         assertNull(calibrator.getEstimatedBiasesAsMatrix());
26463         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26464         assertNull(calibrator.getEstimatedBiasFx());
26465         assertNull(calibrator.getEstimatedBiasFy());
26466         assertNull(calibrator.getEstimatedBiasFz());
26467         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26468         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26469         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26470         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26471         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26472         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26473         assertNull(calibrator.getEstimatedMa());
26474         assertNull(calibrator.getEstimatedSx());
26475         assertNull(calibrator.getEstimatedSy());
26476         assertNull(calibrator.getEstimatedSz());
26477         assertNull(calibrator.getEstimatedMxy());
26478         assertNull(calibrator.getEstimatedMxz());
26479         assertNull(calibrator.getEstimatedMyx());
26480         assertNull(calibrator.getEstimatedMyz());
26481         assertNull(calibrator.getEstimatedMzx());
26482         assertNull(calibrator.getEstimatedMzy());
26483         assertNull(calibrator.getEstimatedCovariance());
26484         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26485         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26486                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26487         assertNotNull(calibrator.getGroundTruthGravityNorm());
26488         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26489         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26490         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26491                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26492         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26493         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26494         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26495     }
26496 
26497     @Test
26498     public void testConstructor206() throws WrongSizeException {
26499         final Collection<StandardDeviationBodyKinematics> measurements =
26500                 Collections.emptyList();
26501 
26502         final Matrix ba = generateBa();
26503         final double biasX = ba.getElementAtIndex(0);
26504         final double biasY = ba.getElementAtIndex(1);
26505         final double biasZ = ba.getElementAtIndex(2);
26506 
26507         final Acceleration bx = new Acceleration(biasX,
26508                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26509         final Acceleration by = new Acceleration(biasY,
26510                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26511         final Acceleration bz = new Acceleration(biasZ,
26512                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26513 
26514         final Matrix ma = generateMaCommonAxis();
26515         final double sx = ma.getElementAt(0, 0);
26516         final double sy = ma.getElementAt(1, 1);
26517         final double sz = ma.getElementAt(2, 2);
26518         final double mxy = ma.getElementAt(0, 1);
26519         final double mxz = ma.getElementAt(0, 2);
26520         final double myx = ma.getElementAt(1, 0);
26521         final double myz = ma.getElementAt(1, 2);
26522         final double mzx = ma.getElementAt(2, 0);
26523         final double mzy = ma.getElementAt(2, 1);
26524 
26525         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26526         final double latitude = Math.toRadians(
26527                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26528         final double longitude = Math.toRadians(
26529                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26530         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26531         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26532         final NEDVelocity nedVelocity = new NEDVelocity();
26533         final ECEFPosition ecefPosition = new ECEFPosition();
26534         final ECEFVelocity ecefVelocity = new ECEFVelocity();
26535         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26536                 ecefPosition, ecefVelocity);
26537 
26538         final KnownPositionAccelerometerCalibrator calibrator =
26539                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
26540                         bx, by, bz, sx, sy, sz, mxy, mxz,
26541                         myx, myz, mzx, mzy, this);
26542 
26543         // check default values
26544         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26545         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26546         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26547         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26548         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26549         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26550         final Acceleration bx2 = new Acceleration(0.0,
26551                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26552         calibrator.getInitialBiasXAsAcceleration(bx2);
26553         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26554         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26555         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26556         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26557         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26558         final Acceleration by2 = new Acceleration(0.0,
26559                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26560         calibrator.getInitialBiasYAsAcceleration(by2);
26561         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26562         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26563         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26564         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26565         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26566         final Acceleration bz2 = new Acceleration(0.0,
26567                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26568         calibrator.getInitialBiasZAsAcceleration(bz2);
26569         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26570         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26571         assertEquals(calibrator.getInitialSx(), sx, 0.0);
26572         assertEquals(calibrator.getInitialSy(), sy, 0.0);
26573         assertEquals(calibrator.getInitialSz(), sz, 0.0);
26574         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
26575         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
26576         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
26577         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
26578         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
26579         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
26580         final double[] bias1 = calibrator.getInitialBias();
26581         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
26582         final double[] bias2 = new double[3];
26583         calibrator.getInitialBias(bias2);
26584         assertArrayEquals(bias1, bias2, 0.0);
26585         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26586         assertEquals(b1, ba);
26587         final Matrix b2 = new Matrix(3, 1);
26588         calibrator.getInitialBiasAsMatrix(b2);
26589         assertEquals(b1, b2);
26590         final Matrix ma1 = new Matrix(3, 3);
26591         ma1.setSubmatrix(0, 0,
26592                 2, 2,
26593                 new double[]{sx, myx, mzx,
26594                         mxy, sy, mzy,
26595                         mxz, myz, sz});
26596         assertEquals(calibrator.getInitialMa(), ma1);
26597         final Matrix ma2 = new Matrix(3, 3);
26598         calibrator.getInitialMa(ma2);
26599         assertEquals(ma1, ma2);
26600         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26601         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26602         final NEDPosition nedPosition1 = new NEDPosition();
26603         assertTrue(calibrator.getNedPosition(nedPosition1));
26604         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26605         assertSame(calibrator.getMeasurements(), measurements);
26606         assertFalse(calibrator.isCommonAxisUsed());
26607         assertSame(calibrator.getListener(), this);
26608         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
26609         assertFalse(calibrator.isReady());
26610         assertFalse(calibrator.isRunning());
26611         assertNull(calibrator.getEstimatedBiases());
26612         assertFalse(calibrator.getEstimatedBiases(null));
26613         assertNull(calibrator.getEstimatedBiasesAsMatrix());
26614         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26615         assertNull(calibrator.getEstimatedBiasFx());
26616         assertNull(calibrator.getEstimatedBiasFy());
26617         assertNull(calibrator.getEstimatedBiasFz());
26618         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26619         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26620         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26621         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26622         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26623         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26624         assertNull(calibrator.getEstimatedMa());
26625         assertNull(calibrator.getEstimatedSx());
26626         assertNull(calibrator.getEstimatedSy());
26627         assertNull(calibrator.getEstimatedSz());
26628         assertNull(calibrator.getEstimatedMxy());
26629         assertNull(calibrator.getEstimatedMxz());
26630         assertNull(calibrator.getEstimatedMyx());
26631         assertNull(calibrator.getEstimatedMyz());
26632         assertNull(calibrator.getEstimatedMzx());
26633         assertNull(calibrator.getEstimatedMzy());
26634         assertNull(calibrator.getEstimatedCovariance());
26635         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26636         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26637                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26638         assertNotNull(calibrator.getGroundTruthGravityNorm());
26639         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26640         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26641         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26642                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26643         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26644         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26645         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26646     }
26647 
26648     @Test
26649     public void testConstructor207() throws WrongSizeException {
26650         final Matrix ba = generateBa();
26651         final double biasX = ba.getElementAtIndex(0);
26652         final double biasY = ba.getElementAtIndex(1);
26653         final double biasZ = ba.getElementAtIndex(2);
26654 
26655         final Acceleration bx = new Acceleration(biasX,
26656                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26657         final Acceleration by = new Acceleration(biasY,
26658                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26659         final Acceleration bz = new Acceleration(biasZ,
26660                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26661 
26662         final Matrix ma = generateMaCommonAxis();
26663         final double sx = ma.getElementAt(0, 0);
26664         final double sy = ma.getElementAt(1, 1);
26665         final double sz = ma.getElementAt(2, 2);
26666         final double mxy = ma.getElementAt(0, 1);
26667         final double mxz = ma.getElementAt(0, 2);
26668         final double myx = ma.getElementAt(1, 0);
26669         final double myz = ma.getElementAt(1, 2);
26670         final double mzx = ma.getElementAt(2, 0);
26671         final double mzy = ma.getElementAt(2, 1);
26672 
26673         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26674         final double latitude = Math.toRadians(
26675                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26676         final double longitude = Math.toRadians(
26677                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26678         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26679         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26680         final NEDVelocity nedVelocity = new NEDVelocity();
26681         final ECEFPosition ecefPosition = new ECEFPosition();
26682         final ECEFVelocity ecefVelocity = new ECEFVelocity();
26683         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26684                 ecefPosition, ecefVelocity);
26685 
26686         final KnownPositionAccelerometerCalibrator calibrator =
26687                 new KnownPositionAccelerometerCalibrator(nedPosition,
26688                         true, bx, by, bz, sx, sy, sz, mxy, mxz,
26689                         myx, myz, mzx, mzy);
26690 
26691         // check default values
26692         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26693         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26694         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26695         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26696         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26697         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26698         final Acceleration bx2 = new Acceleration(0.0,
26699                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26700         calibrator.getInitialBiasXAsAcceleration(bx2);
26701         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26702         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26703         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26704         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26705         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26706         final Acceleration by2 = new Acceleration(0.0,
26707                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26708         calibrator.getInitialBiasYAsAcceleration(by2);
26709         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26710         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26711         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26712         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26713         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26714         final Acceleration bz2 = new Acceleration(0.0,
26715                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26716         calibrator.getInitialBiasZAsAcceleration(bz2);
26717         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26718         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26719         assertEquals(calibrator.getInitialSx(), sx, 0.0);
26720         assertEquals(calibrator.getInitialSy(), sy, 0.0);
26721         assertEquals(calibrator.getInitialSz(), sz, 0.0);
26722         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
26723         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
26724         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
26725         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
26726         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
26727         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
26728         final double[] bias1 = calibrator.getInitialBias();
26729         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
26730         final double[] bias2 = new double[3];
26731         calibrator.getInitialBias(bias2);
26732         assertArrayEquals(bias1, bias2, 0.0);
26733         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26734         assertEquals(b1, ba);
26735         final Matrix b2 = new Matrix(3, 1);
26736         calibrator.getInitialBiasAsMatrix(b2);
26737         assertEquals(b1, b2);
26738         final Matrix ma1 = new Matrix(3, 3);
26739         ma1.setSubmatrix(0, 0,
26740                 2, 2,
26741                 new double[]{sx, myx, mzx,
26742                         mxy, sy, mzy,
26743                         mxz, myz, sz});
26744         assertEquals(calibrator.getInitialMa(), ma1);
26745         final Matrix ma2 = new Matrix(3, 3);
26746         calibrator.getInitialMa(ma2);
26747         assertEquals(ma1, ma2);
26748         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26749         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26750         final NEDPosition nedPosition1 = new NEDPosition();
26751         assertTrue(calibrator.getNedPosition(nedPosition1));
26752         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26753         assertNull(calibrator.getMeasurements());
26754         assertTrue(calibrator.isCommonAxisUsed());
26755         assertNull(calibrator.getListener());
26756         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
26757         assertFalse(calibrator.isReady());
26758         assertFalse(calibrator.isRunning());
26759         assertNull(calibrator.getEstimatedBiases());
26760         assertFalse(calibrator.getEstimatedBiases(null));
26761         assertNull(calibrator.getEstimatedBiasesAsMatrix());
26762         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26763         assertNull(calibrator.getEstimatedBiasFx());
26764         assertNull(calibrator.getEstimatedBiasFy());
26765         assertNull(calibrator.getEstimatedBiasFz());
26766         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26767         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26768         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26769         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26770         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26771         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26772         assertNull(calibrator.getEstimatedMa());
26773         assertNull(calibrator.getEstimatedSx());
26774         assertNull(calibrator.getEstimatedSy());
26775         assertNull(calibrator.getEstimatedSz());
26776         assertNull(calibrator.getEstimatedMxy());
26777         assertNull(calibrator.getEstimatedMxz());
26778         assertNull(calibrator.getEstimatedMyx());
26779         assertNull(calibrator.getEstimatedMyz());
26780         assertNull(calibrator.getEstimatedMzx());
26781         assertNull(calibrator.getEstimatedMzy());
26782         assertNull(calibrator.getEstimatedCovariance());
26783         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26784         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26785                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26786         assertNotNull(calibrator.getGroundTruthGravityNorm());
26787         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26788         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26789         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26790                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26791         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26792         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26793         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26794     }
26795 
26796     @Test
26797     public void testConstructor208() throws WrongSizeException {
26798         final Matrix ba = generateBa();
26799         final double biasX = ba.getElementAtIndex(0);
26800         final double biasY = ba.getElementAtIndex(1);
26801         final double biasZ = ba.getElementAtIndex(2);
26802 
26803         final Acceleration bx = new Acceleration(biasX,
26804                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26805         final Acceleration by = new Acceleration(biasY,
26806                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26807         final Acceleration bz = new Acceleration(biasZ,
26808                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26809 
26810         final Matrix ma = generateMaCommonAxis();
26811         final double sx = ma.getElementAt(0, 0);
26812         final double sy = ma.getElementAt(1, 1);
26813         final double sz = ma.getElementAt(2, 2);
26814         final double mxy = ma.getElementAt(0, 1);
26815         final double mxz = ma.getElementAt(0, 2);
26816         final double myx = ma.getElementAt(1, 0);
26817         final double myz = ma.getElementAt(1, 2);
26818         final double mzx = ma.getElementAt(2, 0);
26819         final double mzy = ma.getElementAt(2, 1);
26820 
26821         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26822         final double latitude = Math.toRadians(
26823                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26824         final double longitude = Math.toRadians(
26825                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26826         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26827         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26828         final NEDVelocity nedVelocity = new NEDVelocity();
26829         final ECEFPosition ecefPosition = new ECEFPosition();
26830         final ECEFVelocity ecefVelocity = new ECEFVelocity();
26831         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26832                 ecefPosition, ecefVelocity);
26833 
26834         final KnownPositionAccelerometerCalibrator calibrator =
26835                 new KnownPositionAccelerometerCalibrator(nedPosition,
26836                         true, bx, by, bz, sx, sy, sz, mxy, mxz,
26837                         myx, myz, mzx, mzy, this);
26838 
26839         // check default values
26840         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26841         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26842         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26843         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26844         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26845         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26846         final Acceleration bx2 = new Acceleration(0.0,
26847                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26848         calibrator.getInitialBiasXAsAcceleration(bx2);
26849         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26850         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26851         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26852         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26853         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26854         final Acceleration by2 = new Acceleration(0.0,
26855                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26856         calibrator.getInitialBiasYAsAcceleration(by2);
26857         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26858         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26859         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26860         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26861         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26862         final Acceleration bz2 = new Acceleration(0.0,
26863                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26864         calibrator.getInitialBiasZAsAcceleration(bz2);
26865         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26866         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26867         assertEquals(calibrator.getInitialSx(), sx, 0.0);
26868         assertEquals(calibrator.getInitialSy(), sy, 0.0);
26869         assertEquals(calibrator.getInitialSz(), sz, 0.0);
26870         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
26871         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
26872         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
26873         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
26874         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
26875         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
26876         final double[] bias1 = calibrator.getInitialBias();
26877         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
26878         final double[] bias2 = new double[3];
26879         calibrator.getInitialBias(bias2);
26880         assertArrayEquals(bias1, bias2, 0.0);
26881         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26882         assertEquals(b1, ba);
26883         final Matrix b2 = new Matrix(3, 1);
26884         calibrator.getInitialBiasAsMatrix(b2);
26885         assertEquals(b1, b2);
26886         final Matrix ma1 = new Matrix(3, 3);
26887         ma1.setSubmatrix(0, 0,
26888                 2, 2,
26889                 new double[]{sx, myx, mzx,
26890                         mxy, sy, mzy,
26891                         mxz, myz, sz});
26892         assertEquals(calibrator.getInitialMa(), ma1);
26893         final Matrix ma2 = new Matrix(3, 3);
26894         calibrator.getInitialMa(ma2);
26895         assertEquals(ma1, ma2);
26896         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26897         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26898         final NEDPosition nedPosition1 = new NEDPosition();
26899         assertTrue(calibrator.getNedPosition(nedPosition1));
26900         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26901         assertNull(calibrator.getMeasurements());
26902         assertTrue(calibrator.isCommonAxisUsed());
26903         assertSame(calibrator.getListener(), this);
26904         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
26905         assertFalse(calibrator.isReady());
26906         assertFalse(calibrator.isRunning());
26907         assertNull(calibrator.getEstimatedBiases());
26908         assertFalse(calibrator.getEstimatedBiases(null));
26909         assertNull(calibrator.getEstimatedBiasesAsMatrix());
26910         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26911         assertNull(calibrator.getEstimatedBiasFx());
26912         assertNull(calibrator.getEstimatedBiasFy());
26913         assertNull(calibrator.getEstimatedBiasFz());
26914         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26915         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26916         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26917         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26918         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26919         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26920         assertNull(calibrator.getEstimatedMa());
26921         assertNull(calibrator.getEstimatedSx());
26922         assertNull(calibrator.getEstimatedSy());
26923         assertNull(calibrator.getEstimatedSz());
26924         assertNull(calibrator.getEstimatedMxy());
26925         assertNull(calibrator.getEstimatedMxz());
26926         assertNull(calibrator.getEstimatedMyx());
26927         assertNull(calibrator.getEstimatedMyz());
26928         assertNull(calibrator.getEstimatedMzx());
26929         assertNull(calibrator.getEstimatedMzy());
26930         assertNull(calibrator.getEstimatedCovariance());
26931         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26932         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26933                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26934         assertNotNull(calibrator.getGroundTruthGravityNorm());
26935         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26936         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26937         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26938                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26939         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26940         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26941         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26942     }
26943 
26944     @Test
26945     public void testConstructor209() throws WrongSizeException {
26946         final Collection<StandardDeviationBodyKinematics> measurements =
26947                 Collections.emptyList();
26948 
26949         final Matrix ba = generateBa();
26950         final double biasX = ba.getElementAtIndex(0);
26951         final double biasY = ba.getElementAtIndex(1);
26952         final double biasZ = ba.getElementAtIndex(2);
26953 
26954         final Acceleration bx = new Acceleration(biasX,
26955                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26956         final Acceleration by = new Acceleration(biasY,
26957                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26958         final Acceleration bz = new Acceleration(biasZ,
26959                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26960 
26961         final Matrix ma = generateMaCommonAxis();
26962         final double sx = ma.getElementAt(0, 0);
26963         final double sy = ma.getElementAt(1, 1);
26964         final double sz = ma.getElementAt(2, 2);
26965         final double mxy = ma.getElementAt(0, 1);
26966         final double mxz = ma.getElementAt(0, 2);
26967         final double myx = ma.getElementAt(1, 0);
26968         final double myz = ma.getElementAt(1, 2);
26969         final double mzx = ma.getElementAt(2, 0);
26970         final double mzy = ma.getElementAt(2, 1);
26971 
26972         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26973         final double latitude = Math.toRadians(
26974                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26975         final double longitude = Math.toRadians(
26976                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26977         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26978         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26979         final NEDVelocity nedVelocity = new NEDVelocity();
26980         final ECEFPosition ecefPosition = new ECEFPosition();
26981         final ECEFVelocity ecefVelocity = new ECEFVelocity();
26982         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26983                 ecefPosition, ecefVelocity);
26984 
26985         final KnownPositionAccelerometerCalibrator calibrator =
26986                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
26987                         true, bx, by, bz, sx, sy, sz, mxy, mxz,
26988                         myx, myz, mzx, mzy);
26989 
26990         // check default values
26991         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26992         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26993         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26994         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26995         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26996         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26997         final Acceleration bx2 = new Acceleration(0.0,
26998                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26999         calibrator.getInitialBiasXAsAcceleration(bx2);
27000         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27001         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27002         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27003         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27004         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27005         final Acceleration by2 = new Acceleration(0.0,
27006                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27007         calibrator.getInitialBiasYAsAcceleration(by2);
27008         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27009         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27010         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27011         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27012         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27013         final Acceleration bz2 = new Acceleration(0.0,
27014                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27015         calibrator.getInitialBiasZAsAcceleration(bz2);
27016         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27017         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27018         assertEquals(calibrator.getInitialSx(), sx, 0.0);
27019         assertEquals(calibrator.getInitialSy(), sy, 0.0);
27020         assertEquals(calibrator.getInitialSz(), sz, 0.0);
27021         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
27022         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
27023         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
27024         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
27025         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
27026         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
27027         final double[] bias1 = calibrator.getInitialBias();
27028         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
27029         final double[] bias2 = new double[3];
27030         calibrator.getInitialBias(bias2);
27031         assertArrayEquals(bias1, bias2, 0.0);
27032         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27033         assertEquals(b1, ba);
27034         final Matrix b2 = new Matrix(3, 1);
27035         calibrator.getInitialBiasAsMatrix(b2);
27036         assertEquals(b1, b2);
27037         final Matrix ma1 = new Matrix(3, 3);
27038         ma1.setSubmatrix(0, 0,
27039                 2, 2,
27040                 new double[]{sx, myx, mzx,
27041                         mxy, sy, mzy,
27042                         mxz, myz, sz});
27043         assertEquals(calibrator.getInitialMa(), ma1);
27044         final Matrix ma2 = new Matrix(3, 3);
27045         calibrator.getInitialMa(ma2);
27046         assertEquals(ma1, ma2);
27047         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27048         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27049         final NEDPosition nedPosition1 = new NEDPosition();
27050         assertTrue(calibrator.getNedPosition(nedPosition1));
27051         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27052         assertSame(calibrator.getMeasurements(), measurements);
27053         assertTrue(calibrator.isCommonAxisUsed());
27054         assertNull(calibrator.getListener());
27055         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
27056         assertFalse(calibrator.isReady());
27057         assertFalse(calibrator.isRunning());
27058         assertNull(calibrator.getEstimatedBiases());
27059         assertFalse(calibrator.getEstimatedBiases(null));
27060         assertNull(calibrator.getEstimatedBiasesAsMatrix());
27061         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27062         assertNull(calibrator.getEstimatedBiasFx());
27063         assertNull(calibrator.getEstimatedBiasFy());
27064         assertNull(calibrator.getEstimatedBiasFz());
27065         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27066         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27067         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27068         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27069         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27070         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27071         assertNull(calibrator.getEstimatedMa());
27072         assertNull(calibrator.getEstimatedSx());
27073         assertNull(calibrator.getEstimatedSy());
27074         assertNull(calibrator.getEstimatedSz());
27075         assertNull(calibrator.getEstimatedMxy());
27076         assertNull(calibrator.getEstimatedMxz());
27077         assertNull(calibrator.getEstimatedMyx());
27078         assertNull(calibrator.getEstimatedMyz());
27079         assertNull(calibrator.getEstimatedMzx());
27080         assertNull(calibrator.getEstimatedMzy());
27081         assertNull(calibrator.getEstimatedCovariance());
27082         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27083         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27084                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27085         assertNotNull(calibrator.getGroundTruthGravityNorm());
27086         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27087         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27088         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27089                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27090         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27091         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27092         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27093     }
27094 
27095     @Test
27096     public void testConstructor210() throws WrongSizeException {
27097         final Collection<StandardDeviationBodyKinematics> measurements =
27098                 Collections.emptyList();
27099 
27100         final Matrix ba = generateBa();
27101         final double biasX = ba.getElementAtIndex(0);
27102         final double biasY = ba.getElementAtIndex(1);
27103         final double biasZ = ba.getElementAtIndex(2);
27104 
27105         final Acceleration bx = new Acceleration(biasX,
27106                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27107         final Acceleration by = new Acceleration(biasY,
27108                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27109         final Acceleration bz = new Acceleration(biasZ,
27110                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27111 
27112         final Matrix ma = generateMaCommonAxis();
27113         final double sx = ma.getElementAt(0, 0);
27114         final double sy = ma.getElementAt(1, 1);
27115         final double sz = ma.getElementAt(2, 2);
27116         final double mxy = ma.getElementAt(0, 1);
27117         final double mxz = ma.getElementAt(0, 2);
27118         final double myx = ma.getElementAt(1, 0);
27119         final double myz = ma.getElementAt(1, 2);
27120         final double mzx = ma.getElementAt(2, 0);
27121         final double mzy = ma.getElementAt(2, 1);
27122 
27123         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27124         final double latitude = Math.toRadians(
27125                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27126         final double longitude = Math.toRadians(
27127                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27128         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27129         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27130         final NEDVelocity nedVelocity = new NEDVelocity();
27131         final ECEFPosition ecefPosition = new ECEFPosition();
27132         final ECEFVelocity ecefVelocity = new ECEFVelocity();
27133         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27134                 ecefPosition, ecefVelocity);
27135 
27136         final KnownPositionAccelerometerCalibrator calibrator =
27137                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
27138                         true, bx, by, bz, sx, sy, sz, mxy, mxz,
27139                         myx, myz, mzx, mzy, this);
27140 
27141         // check default values
27142         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27143         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27144         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27145         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27146         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27147         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27148         final Acceleration bx2 = new Acceleration(0.0,
27149                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27150         calibrator.getInitialBiasXAsAcceleration(bx2);
27151         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27152         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27153         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27154         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27155         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27156         final Acceleration by2 = new Acceleration(0.0,
27157                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27158         calibrator.getInitialBiasYAsAcceleration(by2);
27159         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27160         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27161         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27162         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27163         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27164         final Acceleration bz2 = new Acceleration(0.0,
27165                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27166         calibrator.getInitialBiasZAsAcceleration(bz2);
27167         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27168         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27169         assertEquals(calibrator.getInitialSx(), sx, 0.0);
27170         assertEquals(calibrator.getInitialSy(), sy, 0.0);
27171         assertEquals(calibrator.getInitialSz(), sz, 0.0);
27172         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
27173         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
27174         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
27175         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
27176         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
27177         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
27178         final double[] bias1 = calibrator.getInitialBias();
27179         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
27180         final double[] bias2 = new double[3];
27181         calibrator.getInitialBias(bias2);
27182         assertArrayEquals(bias1, bias2, 0.0);
27183         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27184         assertEquals(b1, ba);
27185         final Matrix b2 = new Matrix(3, 1);
27186         calibrator.getInitialBiasAsMatrix(b2);
27187         assertEquals(b1, b2);
27188         final Matrix ma1 = new Matrix(3, 3);
27189         ma1.setSubmatrix(0, 0,
27190                 2, 2,
27191                 new double[]{sx, myx, mzx,
27192                         mxy, sy, mzy,
27193                         mxz, myz, sz});
27194         assertEquals(calibrator.getInitialMa(), ma1);
27195         final Matrix ma2 = new Matrix(3, 3);
27196         calibrator.getInitialMa(ma2);
27197         assertEquals(ma1, ma2);
27198         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27199         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27200         final NEDPosition nedPosition1 = new NEDPosition();
27201         assertTrue(calibrator.getNedPosition(nedPosition1));
27202         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27203         assertSame(calibrator.getMeasurements(), measurements);
27204         assertTrue(calibrator.isCommonAxisUsed());
27205         assertSame(calibrator.getListener(), this);
27206         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
27207         assertFalse(calibrator.isReady());
27208         assertFalse(calibrator.isRunning());
27209         assertNull(calibrator.getEstimatedBiases());
27210         assertFalse(calibrator.getEstimatedBiases(null));
27211         assertNull(calibrator.getEstimatedBiasesAsMatrix());
27212         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27213         assertNull(calibrator.getEstimatedBiasFx());
27214         assertNull(calibrator.getEstimatedBiasFy());
27215         assertNull(calibrator.getEstimatedBiasFz());
27216         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27217         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27218         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27219         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27220         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27221         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27222         assertNull(calibrator.getEstimatedMa());
27223         assertNull(calibrator.getEstimatedSx());
27224         assertNull(calibrator.getEstimatedSy());
27225         assertNull(calibrator.getEstimatedSz());
27226         assertNull(calibrator.getEstimatedMxy());
27227         assertNull(calibrator.getEstimatedMxz());
27228         assertNull(calibrator.getEstimatedMyx());
27229         assertNull(calibrator.getEstimatedMyz());
27230         assertNull(calibrator.getEstimatedMzx());
27231         assertNull(calibrator.getEstimatedMzy());
27232         assertNull(calibrator.getEstimatedCovariance());
27233         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27234         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27235                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27236         assertNotNull(calibrator.getGroundTruthGravityNorm());
27237         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27238         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27239         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27240                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27241         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27242         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27243         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27244     }
27245 
27246     @Test
27247     public void testConstructor211() throws WrongSizeException {
27248         final Matrix ba = generateBa();
27249         final double[] bias = ba.getBuffer();
27250         final double biasX = ba.getElementAtIndex(0);
27251         final double biasY = ba.getElementAtIndex(1);
27252         final double biasZ = ba.getElementAtIndex(2);
27253 
27254         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27255         final double latitude = Math.toRadians(
27256                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27257         final double longitude = Math.toRadians(
27258                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27259         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27260         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27261         final NEDVelocity nedVelocity = new NEDVelocity();
27262         final ECEFPosition ecefPosition = new ECEFPosition();
27263         final ECEFVelocity ecefVelocity = new ECEFVelocity();
27264         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27265                 ecefPosition, ecefVelocity);
27266 
27267         KnownPositionAccelerometerCalibrator calibrator =
27268                 new KnownPositionAccelerometerCalibrator(nedPosition, bias);
27269 
27270         // check default values
27271         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27272         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27273         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27274         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27275         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27276         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27277         final Acceleration bx2 = new Acceleration(0.0,
27278                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27279         calibrator.getInitialBiasXAsAcceleration(bx2);
27280         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27281         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27282         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27283         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27284         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27285         final Acceleration by2 = new Acceleration(0.0,
27286                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27287         calibrator.getInitialBiasYAsAcceleration(by2);
27288         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27289         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27290         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27291         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27292         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27293         final Acceleration bz2 = new Acceleration(0.0,
27294                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27295         calibrator.getInitialBiasZAsAcceleration(bz2);
27296         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27297         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27298         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27299         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27300         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27301         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27302         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27303         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27304         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27305         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27306         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27307         final double[] bias1 = calibrator.getInitialBias();
27308         assertArrayEquals(bias1, bias, 0.0);
27309         final double[] bias2 = new double[3];
27310         calibrator.getInitialBias(bias2);
27311         assertArrayEquals(bias1, bias2, 0.0);
27312         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27313         assertEquals(b1, ba);
27314         final Matrix b2 = new Matrix(3, 1);
27315         calibrator.getInitialBiasAsMatrix(b2);
27316         assertEquals(b1, b2);
27317         final Matrix ma1 = calibrator.getInitialMa();
27318         assertEquals(ma1, new Matrix(3, 3));
27319         final Matrix ma2 = new Matrix(3, 3);
27320         calibrator.getInitialMa(ma2);
27321         assertEquals(ma1, ma2);
27322         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27323         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27324         final NEDPosition nedPosition1 = new NEDPosition();
27325         assertTrue(calibrator.getNedPosition(nedPosition1));
27326         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27327         assertNull(calibrator.getMeasurements());
27328         assertFalse(calibrator.isCommonAxisUsed());
27329         assertNull(calibrator.getListener());
27330         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
27331         assertFalse(calibrator.isReady());
27332         assertFalse(calibrator.isRunning());
27333         assertNull(calibrator.getEstimatedBiases());
27334         assertFalse(calibrator.getEstimatedBiases(null));
27335         assertNull(calibrator.getEstimatedBiasesAsMatrix());
27336         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27337         assertNull(calibrator.getEstimatedBiasFx());
27338         assertNull(calibrator.getEstimatedBiasFy());
27339         assertNull(calibrator.getEstimatedBiasFz());
27340         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27341         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27342         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27343         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27344         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27345         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27346         assertNull(calibrator.getEstimatedMa());
27347         assertNull(calibrator.getEstimatedSx());
27348         assertNull(calibrator.getEstimatedSy());
27349         assertNull(calibrator.getEstimatedSz());
27350         assertNull(calibrator.getEstimatedMxy());
27351         assertNull(calibrator.getEstimatedMxz());
27352         assertNull(calibrator.getEstimatedMyx());
27353         assertNull(calibrator.getEstimatedMyz());
27354         assertNull(calibrator.getEstimatedMzx());
27355         assertNull(calibrator.getEstimatedMzy());
27356         assertNull(calibrator.getEstimatedCovariance());
27357         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27358         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27359                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27360         assertNotNull(calibrator.getGroundTruthGravityNorm());
27361         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27362         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27363         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27364                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27365         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27366         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27367         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27368 
27369         // Force IllegalArgumentException
27370         calibrator = null;
27371         try {
27372             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27373                     new double[1]);
27374             fail("IllegalArgumentException expected but not thrown");
27375         } catch (final IllegalArgumentException ignore) {
27376         }
27377         assertNull(calibrator);
27378     }
27379 
27380     @Test
27381     public void testConstructor212() throws WrongSizeException {
27382         final Matrix ba = generateBa();
27383         final double[] bias = ba.getBuffer();
27384         final double biasX = ba.getElementAtIndex(0);
27385         final double biasY = ba.getElementAtIndex(1);
27386         final double biasZ = ba.getElementAtIndex(2);
27387 
27388         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27389         final double latitude = Math.toRadians(
27390                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27391         final double longitude = Math.toRadians(
27392                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27393         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27394         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27395         final NEDVelocity nedVelocity = new NEDVelocity();
27396         final ECEFPosition ecefPosition = new ECEFPosition();
27397         final ECEFVelocity ecefVelocity = new ECEFVelocity();
27398         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27399                 ecefPosition, ecefVelocity);
27400 
27401         KnownPositionAccelerometerCalibrator calibrator =
27402                 new KnownPositionAccelerometerCalibrator(nedPosition, bias,
27403                         this);
27404 
27405         // check default values
27406         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27407         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27408         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27409         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27410         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27411         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27412         final Acceleration bx2 = new Acceleration(0.0,
27413                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27414         calibrator.getInitialBiasXAsAcceleration(bx2);
27415         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27416         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27417         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27418         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27419         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27420         final Acceleration by2 = new Acceleration(0.0,
27421                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27422         calibrator.getInitialBiasYAsAcceleration(by2);
27423         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27424         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27425         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27426         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27427         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27428         final Acceleration bz2 = new Acceleration(0.0,
27429                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27430         calibrator.getInitialBiasZAsAcceleration(bz2);
27431         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27432         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27433         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27434         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27435         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27436         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27437         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27438         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27439         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27440         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27441         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27442         final double[] bias1 = calibrator.getInitialBias();
27443         assertArrayEquals(bias1, bias, 0.0);
27444         final double[] bias2 = new double[3];
27445         calibrator.getInitialBias(bias2);
27446         assertArrayEquals(bias1, bias2, 0.0);
27447         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27448         assertEquals(b1, ba);
27449         final Matrix b2 = new Matrix(3, 1);
27450         calibrator.getInitialBiasAsMatrix(b2);
27451         assertEquals(b1, b2);
27452         final Matrix ma1 = calibrator.getInitialMa();
27453         assertEquals(ma1, new Matrix(3, 3));
27454         final Matrix ma2 = new Matrix(3, 3);
27455         calibrator.getInitialMa(ma2);
27456         assertEquals(ma1, ma2);
27457         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27458         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27459         final NEDPosition nedPosition1 = new NEDPosition();
27460         assertTrue(calibrator.getNedPosition(nedPosition1));
27461         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27462         assertNull(calibrator.getMeasurements());
27463         assertFalse(calibrator.isCommonAxisUsed());
27464         assertSame(calibrator.getListener(), this);
27465         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
27466         assertFalse(calibrator.isReady());
27467         assertFalse(calibrator.isRunning());
27468         assertNull(calibrator.getEstimatedBiases());
27469         assertFalse(calibrator.getEstimatedBiases(null));
27470         assertNull(calibrator.getEstimatedBiasesAsMatrix());
27471         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27472         assertNull(calibrator.getEstimatedBiasFx());
27473         assertNull(calibrator.getEstimatedBiasFy());
27474         assertNull(calibrator.getEstimatedBiasFz());
27475         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27476         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27477         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27478         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27479         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27480         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27481         assertNull(calibrator.getEstimatedMa());
27482         assertNull(calibrator.getEstimatedSx());
27483         assertNull(calibrator.getEstimatedSy());
27484         assertNull(calibrator.getEstimatedSz());
27485         assertNull(calibrator.getEstimatedMxy());
27486         assertNull(calibrator.getEstimatedMxz());
27487         assertNull(calibrator.getEstimatedMyx());
27488         assertNull(calibrator.getEstimatedMyz());
27489         assertNull(calibrator.getEstimatedMzx());
27490         assertNull(calibrator.getEstimatedMzy());
27491         assertNull(calibrator.getEstimatedCovariance());
27492         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27493         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27494                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27495         assertNotNull(calibrator.getGroundTruthGravityNorm());
27496         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27497         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27498         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27499                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27500         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27501         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27502         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27503 
27504         // Force IllegalArgumentException
27505         calibrator = null;
27506         try {
27507             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27508                     new double[1], this);
27509             fail("IllegalArgumentException expected but not thrown");
27510         } catch (final IllegalArgumentException ignore) {
27511         }
27512         assertNull(calibrator);
27513     }
27514 
27515     @Test
27516     public void testConstructor213() throws WrongSizeException {
27517         final Collection<StandardDeviationBodyKinematics> measurements =
27518                 Collections.emptyList();
27519 
27520         final Matrix ba = generateBa();
27521         final double[] bias = ba.getBuffer();
27522         final double biasX = ba.getElementAtIndex(0);
27523         final double biasY = ba.getElementAtIndex(1);
27524         final double biasZ = ba.getElementAtIndex(2);
27525 
27526         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27527         final double latitude = Math.toRadians(
27528                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27529         final double longitude = Math.toRadians(
27530                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27531         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27532         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27533         final NEDVelocity nedVelocity = new NEDVelocity();
27534         final ECEFPosition ecefPosition = new ECEFPosition();
27535         final ECEFVelocity ecefVelocity = new ECEFVelocity();
27536         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27537                 ecefPosition, ecefVelocity);
27538 
27539         KnownPositionAccelerometerCalibrator calibrator =
27540                 new KnownPositionAccelerometerCalibrator(nedPosition,
27541                         measurements, bias);
27542 
27543         // check default values
27544         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27545         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27546         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27547         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27548         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27549         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27550         final Acceleration bx2 = new Acceleration(0.0,
27551                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27552         calibrator.getInitialBiasXAsAcceleration(bx2);
27553         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27554         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27555         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27556         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27557         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27558         final Acceleration by2 = new Acceleration(0.0,
27559                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27560         calibrator.getInitialBiasYAsAcceleration(by2);
27561         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27562         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27563         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27564         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27565         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27566         final Acceleration bz2 = new Acceleration(0.0,
27567                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27568         calibrator.getInitialBiasZAsAcceleration(bz2);
27569         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27570         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27571         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27572         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27573         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27574         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27575         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27576         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27577         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27578         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27579         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27580         final double[] bias1 = calibrator.getInitialBias();
27581         assertArrayEquals(bias1, bias, 0.0);
27582         final double[] bias2 = new double[3];
27583         calibrator.getInitialBias(bias2);
27584         assertArrayEquals(bias1, bias2, 0.0);
27585         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27586         assertEquals(b1, ba);
27587         final Matrix b2 = new Matrix(3, 1);
27588         calibrator.getInitialBiasAsMatrix(b2);
27589         assertEquals(b1, b2);
27590         final Matrix ma1 = calibrator.getInitialMa();
27591         assertEquals(ma1, new Matrix(3, 3));
27592         final Matrix ma2 = new Matrix(3, 3);
27593         calibrator.getInitialMa(ma2);
27594         assertEquals(ma1, ma2);
27595         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27596         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27597         final NEDPosition nedPosition1 = new NEDPosition();
27598         assertTrue(calibrator.getNedPosition(nedPosition1));
27599         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27600         assertSame(calibrator.getMeasurements(), measurements);
27601         assertFalse(calibrator.isCommonAxisUsed());
27602         assertNull(calibrator.getListener());
27603         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
27604         assertFalse(calibrator.isReady());
27605         assertFalse(calibrator.isRunning());
27606         assertNull(calibrator.getEstimatedBiases());
27607         assertFalse(calibrator.getEstimatedBiases(null));
27608         assertNull(calibrator.getEstimatedBiasesAsMatrix());
27609         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27610         assertNull(calibrator.getEstimatedBiasFx());
27611         assertNull(calibrator.getEstimatedBiasFy());
27612         assertNull(calibrator.getEstimatedBiasFz());
27613         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27614         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27615         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27616         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27617         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27618         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27619         assertNull(calibrator.getEstimatedMa());
27620         assertNull(calibrator.getEstimatedSx());
27621         assertNull(calibrator.getEstimatedSy());
27622         assertNull(calibrator.getEstimatedSz());
27623         assertNull(calibrator.getEstimatedMxy());
27624         assertNull(calibrator.getEstimatedMxz());
27625         assertNull(calibrator.getEstimatedMyx());
27626         assertNull(calibrator.getEstimatedMyz());
27627         assertNull(calibrator.getEstimatedMzx());
27628         assertNull(calibrator.getEstimatedMzy());
27629         assertNull(calibrator.getEstimatedCovariance());
27630         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27631         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27632                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27633         assertNotNull(calibrator.getGroundTruthGravityNorm());
27634         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27635         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27636         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27637                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27638         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27639         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27640         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27641 
27642         // Force IllegalArgumentException
27643         calibrator = null;
27644         try {
27645             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27646                     measurements, new double[1]);
27647             fail("IllegalArgumentException expected but not thrown");
27648         } catch (final IllegalArgumentException ignore) {
27649         }
27650         assertNull(calibrator);
27651     }
27652 
27653     @Test
27654     public void testConstructor214() throws WrongSizeException {
27655         final Collection<StandardDeviationBodyKinematics> measurements =
27656                 Collections.emptyList();
27657 
27658         final Matrix ba = generateBa();
27659         final double[] bias = ba.getBuffer();
27660         final double biasX = ba.getElementAtIndex(0);
27661         final double biasY = ba.getElementAtIndex(1);
27662         final double biasZ = ba.getElementAtIndex(2);
27663 
27664         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27665         final double latitude = Math.toRadians(
27666                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27667         final double longitude = Math.toRadians(
27668                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27669         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27670         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27671         final NEDVelocity nedVelocity = new NEDVelocity();
27672         final ECEFPosition ecefPosition = new ECEFPosition();
27673         final ECEFVelocity ecefVelocity = new ECEFVelocity();
27674         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27675                 ecefPosition, ecefVelocity);
27676 
27677         KnownPositionAccelerometerCalibrator calibrator =
27678                 new KnownPositionAccelerometerCalibrator(nedPosition,
27679                         measurements, bias, this);
27680 
27681         // check default values
27682         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27683         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27684         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27685         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27686         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27687         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27688         final Acceleration bx2 = new Acceleration(0.0,
27689                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27690         calibrator.getInitialBiasXAsAcceleration(bx2);
27691         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27692         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27693         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27694         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27695         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27696         final Acceleration by2 = new Acceleration(0.0,
27697                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27698         calibrator.getInitialBiasYAsAcceleration(by2);
27699         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27700         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27701         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27702         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27703         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27704         final Acceleration bz2 = new Acceleration(0.0,
27705                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27706         calibrator.getInitialBiasZAsAcceleration(bz2);
27707         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27708         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27709         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27710         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27711         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27712         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27713         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27714         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27715         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27716         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27717         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27718         final double[] bias1 = calibrator.getInitialBias();
27719         assertArrayEquals(bias1, bias, 0.0);
27720         final double[] bias2 = new double[3];
27721         calibrator.getInitialBias(bias2);
27722         assertArrayEquals(bias1, bias2, 0.0);
27723         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27724         assertEquals(b1, ba);
27725         final Matrix b2 = new Matrix(3, 1);
27726         calibrator.getInitialBiasAsMatrix(b2);
27727         assertEquals(b1, b2);
27728         final Matrix ma1 = calibrator.getInitialMa();
27729         assertEquals(ma1, new Matrix(3, 3));
27730         final Matrix ma2 = new Matrix(3, 3);
27731         calibrator.getInitialMa(ma2);
27732         assertEquals(ma1, ma2);
27733         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27734         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27735         final NEDPosition nedPosition1 = new NEDPosition();
27736         assertTrue(calibrator.getNedPosition(nedPosition1));
27737         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27738         assertSame(calibrator.getMeasurements(), measurements);
27739         assertFalse(calibrator.isCommonAxisUsed());
27740         assertSame(calibrator.getListener(), this);
27741         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
27742         assertFalse(calibrator.isReady());
27743         assertFalse(calibrator.isRunning());
27744         assertNull(calibrator.getEstimatedBiases());
27745         assertFalse(calibrator.getEstimatedBiases(null));
27746         assertNull(calibrator.getEstimatedBiasesAsMatrix());
27747         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27748         assertNull(calibrator.getEstimatedBiasFx());
27749         assertNull(calibrator.getEstimatedBiasFy());
27750         assertNull(calibrator.getEstimatedBiasFz());
27751         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27752         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27753         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27754         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27755         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27756         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27757         assertNull(calibrator.getEstimatedMa());
27758         assertNull(calibrator.getEstimatedSx());
27759         assertNull(calibrator.getEstimatedSy());
27760         assertNull(calibrator.getEstimatedSz());
27761         assertNull(calibrator.getEstimatedMxy());
27762         assertNull(calibrator.getEstimatedMxz());
27763         assertNull(calibrator.getEstimatedMyx());
27764         assertNull(calibrator.getEstimatedMyz());
27765         assertNull(calibrator.getEstimatedMzx());
27766         assertNull(calibrator.getEstimatedMzy());
27767         assertNull(calibrator.getEstimatedCovariance());
27768         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27769         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27770                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27771         assertNotNull(calibrator.getGroundTruthGravityNorm());
27772         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27773         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27774         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27775                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27776         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27777         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27778         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27779 
27780         // Force IllegalArgumentException
27781         calibrator = null;
27782         try {
27783             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27784                     measurements, new double[1], this);
27785             fail("IllegalArgumentException expected but not thrown");
27786         } catch (final IllegalArgumentException ignore) {
27787         }
27788         assertNull(calibrator);
27789     }
27790 
27791     @Test
27792     public void testConstructor215() throws WrongSizeException {
27793         final Matrix ba = generateBa();
27794         final double[] bias = ba.getBuffer();
27795         final double biasX = ba.getElementAtIndex(0);
27796         final double biasY = ba.getElementAtIndex(1);
27797         final double biasZ = ba.getElementAtIndex(2);
27798 
27799         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27800         final double latitude = Math.toRadians(
27801                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27802         final double longitude = Math.toRadians(
27803                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27804         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27805         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27806         final NEDVelocity nedVelocity = new NEDVelocity();
27807         final ECEFPosition ecefPosition = new ECEFPosition();
27808         final ECEFVelocity ecefVelocity = new ECEFVelocity();
27809         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27810                 ecefPosition, ecefVelocity);
27811 
27812         KnownPositionAccelerometerCalibrator calibrator =
27813                 new KnownPositionAccelerometerCalibrator(nedPosition,
27814                         true, bias);
27815 
27816         // check default values
27817         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27818         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27819         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27820         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27821         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27822         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27823         final Acceleration bx2 = new Acceleration(0.0,
27824                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27825         calibrator.getInitialBiasXAsAcceleration(bx2);
27826         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27827         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27828         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27829         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27830         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27831         final Acceleration by2 = new Acceleration(0.0,
27832                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27833         calibrator.getInitialBiasYAsAcceleration(by2);
27834         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27835         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27836         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27837         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27838         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27839         final Acceleration bz2 = new Acceleration(0.0,
27840                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27841         calibrator.getInitialBiasZAsAcceleration(bz2);
27842         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27843         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27844         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27845         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27846         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27847         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27848         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27849         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27850         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27851         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27852         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27853         final double[] bias1 = calibrator.getInitialBias();
27854         assertArrayEquals(bias1, bias, 0.0);
27855         final double[] bias2 = new double[3];
27856         calibrator.getInitialBias(bias2);
27857         assertArrayEquals(bias1, bias2, 0.0);
27858         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27859         assertEquals(b1, ba);
27860         final Matrix b2 = new Matrix(3, 1);
27861         calibrator.getInitialBiasAsMatrix(b2);
27862         assertEquals(b1, b2);
27863         final Matrix ma1 = calibrator.getInitialMa();
27864         assertEquals(ma1, new Matrix(3, 3));
27865         final Matrix ma2 = new Matrix(3, 3);
27866         calibrator.getInitialMa(ma2);
27867         assertEquals(ma1, ma2);
27868         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27869         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27870         final NEDPosition nedPosition1 = new NEDPosition();
27871         assertTrue(calibrator.getNedPosition(nedPosition1));
27872         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27873         assertNull(calibrator.getMeasurements());
27874         assertTrue(calibrator.isCommonAxisUsed());
27875         assertNull(calibrator.getListener());
27876         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
27877         assertFalse(calibrator.isReady());
27878         assertFalse(calibrator.isRunning());
27879         assertNull(calibrator.getEstimatedBiases());
27880         assertFalse(calibrator.getEstimatedBiases(null));
27881         assertNull(calibrator.getEstimatedBiasesAsMatrix());
27882         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27883         assertNull(calibrator.getEstimatedBiasFx());
27884         assertNull(calibrator.getEstimatedBiasFy());
27885         assertNull(calibrator.getEstimatedBiasFz());
27886         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27887         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27888         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27889         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27890         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27891         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27892         assertNull(calibrator.getEstimatedMa());
27893         assertNull(calibrator.getEstimatedSx());
27894         assertNull(calibrator.getEstimatedSy());
27895         assertNull(calibrator.getEstimatedSz());
27896         assertNull(calibrator.getEstimatedMxy());
27897         assertNull(calibrator.getEstimatedMxz());
27898         assertNull(calibrator.getEstimatedMyx());
27899         assertNull(calibrator.getEstimatedMyz());
27900         assertNull(calibrator.getEstimatedMzx());
27901         assertNull(calibrator.getEstimatedMzy());
27902         assertNull(calibrator.getEstimatedCovariance());
27903         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27904         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27905                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27906         assertNotNull(calibrator.getGroundTruthGravityNorm());
27907         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27908         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27909         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27910                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27911         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27912         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27913         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27914 
27915         // Force IllegalArgumentException
27916         calibrator = null;
27917         try {
27918             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27919                     true, new double[1]);
27920             fail("IllegalArgumentException expected but not thrown");
27921         } catch (final IllegalArgumentException ignore) {
27922         }
27923         assertNull(calibrator);
27924     }
27925 
27926     @Test
27927     public void testConstructor216() throws WrongSizeException {
27928         final Matrix ba = generateBa();
27929         final double[] bias = ba.getBuffer();
27930         final double biasX = ba.getElementAtIndex(0);
27931         final double biasY = ba.getElementAtIndex(1);
27932         final double biasZ = ba.getElementAtIndex(2);
27933 
27934         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27935         final double latitude = Math.toRadians(
27936                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27937         final double longitude = Math.toRadians(
27938                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27939         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27940         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27941         final NEDVelocity nedVelocity = new NEDVelocity();
27942         final ECEFPosition ecefPosition = new ECEFPosition();
27943         final ECEFVelocity ecefVelocity = new ECEFVelocity();
27944         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27945                 ecefPosition, ecefVelocity);
27946 
27947         KnownPositionAccelerometerCalibrator calibrator =
27948                 new KnownPositionAccelerometerCalibrator(nedPosition,
27949                         true, bias, this);
27950 
27951         // check default values
27952         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27953         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27954         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27955         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27956         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27957         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27958         final Acceleration bx2 = new Acceleration(0.0,
27959                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27960         calibrator.getInitialBiasXAsAcceleration(bx2);
27961         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27962         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27963         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27964         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27965         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27966         final Acceleration by2 = new Acceleration(0.0,
27967                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27968         calibrator.getInitialBiasYAsAcceleration(by2);
27969         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27970         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27971         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27972         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27973         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27974         final Acceleration bz2 = new Acceleration(0.0,
27975                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27976         calibrator.getInitialBiasZAsAcceleration(bz2);
27977         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27978         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27979         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27980         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27981         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27982         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27983         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27984         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27985         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27986         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27987         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27988         final double[] bias1 = calibrator.getInitialBias();
27989         assertArrayEquals(bias1, bias, 0.0);
27990         final double[] bias2 = new double[3];
27991         calibrator.getInitialBias(bias2);
27992         assertArrayEquals(bias1, bias2, 0.0);
27993         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27994         assertEquals(b1, ba);
27995         final Matrix b2 = new Matrix(3, 1);
27996         calibrator.getInitialBiasAsMatrix(b2);
27997         assertEquals(b1, b2);
27998         final Matrix ma1 = calibrator.getInitialMa();
27999         assertEquals(ma1, new Matrix(3, 3));
28000         final Matrix ma2 = new Matrix(3, 3);
28001         calibrator.getInitialMa(ma2);
28002         assertEquals(ma1, ma2);
28003         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28004         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28005         final NEDPosition nedPosition1 = new NEDPosition();
28006         assertTrue(calibrator.getNedPosition(nedPosition1));
28007         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28008         assertNull(calibrator.getMeasurements());
28009         assertTrue(calibrator.isCommonAxisUsed());
28010         assertSame(calibrator.getListener(), this);
28011         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
28012         assertFalse(calibrator.isReady());
28013         assertFalse(calibrator.isRunning());
28014         assertNull(calibrator.getEstimatedBiases());
28015         assertFalse(calibrator.getEstimatedBiases(null));
28016         assertNull(calibrator.getEstimatedBiasesAsMatrix());
28017         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28018         assertNull(calibrator.getEstimatedBiasFx());
28019         assertNull(calibrator.getEstimatedBiasFy());
28020         assertNull(calibrator.getEstimatedBiasFz());
28021         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28022         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28023         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28024         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28025         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28026         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28027         assertNull(calibrator.getEstimatedMa());
28028         assertNull(calibrator.getEstimatedSx());
28029         assertNull(calibrator.getEstimatedSy());
28030         assertNull(calibrator.getEstimatedSz());
28031         assertNull(calibrator.getEstimatedMxy());
28032         assertNull(calibrator.getEstimatedMxz());
28033         assertNull(calibrator.getEstimatedMyx());
28034         assertNull(calibrator.getEstimatedMyz());
28035         assertNull(calibrator.getEstimatedMzx());
28036         assertNull(calibrator.getEstimatedMzy());
28037         assertNull(calibrator.getEstimatedCovariance());
28038         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28039         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28040                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28041         assertNotNull(calibrator.getGroundTruthGravityNorm());
28042         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28043         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28044         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28045                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28046         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28047         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28048         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28049 
28050         // Force IllegalArgumentException
28051         calibrator = null;
28052         try {
28053             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28054                     true, new double[1], this);
28055             fail("IllegalArgumentException expected but not thrown");
28056         } catch (final IllegalArgumentException ignore) {
28057         }
28058         assertNull(calibrator);
28059     }
28060 
28061     @Test
28062     public void testConstructor217() throws WrongSizeException {
28063         final Collection<StandardDeviationBodyKinematics> measurements =
28064                 Collections.emptyList();
28065 
28066         final Matrix ba = generateBa();
28067         final double[] bias = ba.getBuffer();
28068         final double biasX = ba.getElementAtIndex(0);
28069         final double biasY = ba.getElementAtIndex(1);
28070         final double biasZ = ba.getElementAtIndex(2);
28071 
28072         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28073         final double latitude = Math.toRadians(
28074                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
28075         final double longitude = Math.toRadians(
28076                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28077         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28078         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28079         final NEDVelocity nedVelocity = new NEDVelocity();
28080         final ECEFPosition ecefPosition = new ECEFPosition();
28081         final ECEFVelocity ecefVelocity = new ECEFVelocity();
28082         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28083                 ecefPosition, ecefVelocity);
28084 
28085         KnownPositionAccelerometerCalibrator calibrator =
28086                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
28087                         true, bias);
28088 
28089         // check default values
28090         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28091         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28092         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28093         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28094         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28095         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28096         final Acceleration bx2 = new Acceleration(0.0,
28097                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28098         calibrator.getInitialBiasXAsAcceleration(bx2);
28099         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28100         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28101         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28102         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28103         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28104         final Acceleration by2 = new Acceleration(0.0,
28105                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28106         calibrator.getInitialBiasYAsAcceleration(by2);
28107         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28108         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28109         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28110         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28111         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28112         final Acceleration bz2 = new Acceleration(0.0,
28113                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28114         calibrator.getInitialBiasZAsAcceleration(bz2);
28115         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28116         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28117         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28118         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28119         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28120         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28121         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
28122         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
28123         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
28124         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
28125         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
28126         final double[] bias1 = calibrator.getInitialBias();
28127         assertArrayEquals(bias1, bias, 0.0);
28128         final double[] bias2 = new double[3];
28129         calibrator.getInitialBias(bias2);
28130         assertArrayEquals(bias1, bias2, 0.0);
28131         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28132         assertEquals(b1, ba);
28133         final Matrix b2 = new Matrix(3, 1);
28134         calibrator.getInitialBiasAsMatrix(b2);
28135         assertEquals(b1, b2);
28136         final Matrix ma1 = calibrator.getInitialMa();
28137         assertEquals(ma1, new Matrix(3, 3));
28138         final Matrix ma2 = new Matrix(3, 3);
28139         calibrator.getInitialMa(ma2);
28140         assertEquals(ma1, ma2);
28141         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28142         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28143         final NEDPosition nedPosition1 = new NEDPosition();
28144         assertTrue(calibrator.getNedPosition(nedPosition1));
28145         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28146         assertSame(calibrator.getMeasurements(), measurements);
28147         assertTrue(calibrator.isCommonAxisUsed());
28148         assertNull(calibrator.getListener());
28149         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
28150         assertFalse(calibrator.isReady());
28151         assertFalse(calibrator.isRunning());
28152         assertNull(calibrator.getEstimatedBiases());
28153         assertFalse(calibrator.getEstimatedBiases(null));
28154         assertNull(calibrator.getEstimatedBiasesAsMatrix());
28155         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28156         assertNull(calibrator.getEstimatedBiasFx());
28157         assertNull(calibrator.getEstimatedBiasFy());
28158         assertNull(calibrator.getEstimatedBiasFz());
28159         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28160         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28161         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28162         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28163         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28164         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28165         assertNull(calibrator.getEstimatedMa());
28166         assertNull(calibrator.getEstimatedSx());
28167         assertNull(calibrator.getEstimatedSy());
28168         assertNull(calibrator.getEstimatedSz());
28169         assertNull(calibrator.getEstimatedMxy());
28170         assertNull(calibrator.getEstimatedMxz());
28171         assertNull(calibrator.getEstimatedMyx());
28172         assertNull(calibrator.getEstimatedMyz());
28173         assertNull(calibrator.getEstimatedMzx());
28174         assertNull(calibrator.getEstimatedMzy());
28175         assertNull(calibrator.getEstimatedCovariance());
28176         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28177         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28178                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28179         assertNotNull(calibrator.getGroundTruthGravityNorm());
28180         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28181         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28182         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28183                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28184         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28185         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28186         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28187 
28188         // Force IllegalArgumentException
28189         calibrator = null;
28190         try {
28191             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28192                     measurements, true, new double[1]);
28193             fail("IllegalArgumentException expected but not thrown");
28194         } catch (final IllegalArgumentException ignore) {
28195         }
28196         assertNull(calibrator);
28197     }
28198 
28199     @Test
28200     public void testConstructor218() throws WrongSizeException {
28201         final Collection<StandardDeviationBodyKinematics> measurements =
28202                 Collections.emptyList();
28203 
28204         final Matrix ba = generateBa();
28205         final double[] bias = ba.getBuffer();
28206         final double biasX = ba.getElementAtIndex(0);
28207         final double biasY = ba.getElementAtIndex(1);
28208         final double biasZ = ba.getElementAtIndex(2);
28209 
28210         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28211         final double latitude = Math.toRadians(
28212                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
28213         final double longitude = Math.toRadians(
28214                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28215         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28216         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28217         final NEDVelocity nedVelocity = new NEDVelocity();
28218         final ECEFPosition ecefPosition = new ECEFPosition();
28219         final ECEFVelocity ecefVelocity = new ECEFVelocity();
28220         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28221                 ecefPosition, ecefVelocity);
28222 
28223         KnownPositionAccelerometerCalibrator calibrator =
28224                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
28225                         true, bias, this);
28226 
28227         // check default values
28228         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28229         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28230         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28231         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28232         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28233         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28234         final Acceleration bx2 = new Acceleration(0.0,
28235                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28236         calibrator.getInitialBiasXAsAcceleration(bx2);
28237         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28238         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28239         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28240         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28241         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28242         final Acceleration by2 = new Acceleration(0.0,
28243                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28244         calibrator.getInitialBiasYAsAcceleration(by2);
28245         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28246         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28247         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28248         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28249         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28250         final Acceleration bz2 = new Acceleration(0.0,
28251                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28252         calibrator.getInitialBiasZAsAcceleration(bz2);
28253         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28254         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28255         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28256         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28257         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28258         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28259         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
28260         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
28261         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
28262         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
28263         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
28264         final double[] bias1 = calibrator.getInitialBias();
28265         assertArrayEquals(bias1, bias, 0.0);
28266         final double[] bias2 = new double[3];
28267         calibrator.getInitialBias(bias2);
28268         assertArrayEquals(bias1, bias2, 0.0);
28269         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28270         assertEquals(b1, ba);
28271         final Matrix b2 = new Matrix(3, 1);
28272         calibrator.getInitialBiasAsMatrix(b2);
28273         assertEquals(b1, b2);
28274         final Matrix ma1 = calibrator.getInitialMa();
28275         assertEquals(ma1, new Matrix(3, 3));
28276         final Matrix ma2 = new Matrix(3, 3);
28277         calibrator.getInitialMa(ma2);
28278         assertEquals(ma1, ma2);
28279         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28280         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28281         final NEDPosition nedPosition1 = new NEDPosition();
28282         assertTrue(calibrator.getNedPosition(nedPosition1));
28283         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28284         assertSame(calibrator.getMeasurements(), measurements);
28285         assertTrue(calibrator.isCommonAxisUsed());
28286         assertSame(calibrator.getListener(), this);
28287         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
28288         assertFalse(calibrator.isReady());
28289         assertFalse(calibrator.isRunning());
28290         assertNull(calibrator.getEstimatedBiases());
28291         assertFalse(calibrator.getEstimatedBiases(null));
28292         assertNull(calibrator.getEstimatedBiasesAsMatrix());
28293         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28294         assertNull(calibrator.getEstimatedBiasFx());
28295         assertNull(calibrator.getEstimatedBiasFy());
28296         assertNull(calibrator.getEstimatedBiasFz());
28297         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28298         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28299         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28300         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28301         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28302         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28303         assertNull(calibrator.getEstimatedMa());
28304         assertNull(calibrator.getEstimatedSx());
28305         assertNull(calibrator.getEstimatedSy());
28306         assertNull(calibrator.getEstimatedSz());
28307         assertNull(calibrator.getEstimatedMxy());
28308         assertNull(calibrator.getEstimatedMxz());
28309         assertNull(calibrator.getEstimatedMyx());
28310         assertNull(calibrator.getEstimatedMyz());
28311         assertNull(calibrator.getEstimatedMzx());
28312         assertNull(calibrator.getEstimatedMzy());
28313         assertNull(calibrator.getEstimatedCovariance());
28314         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28315         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28316                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28317         assertNotNull(calibrator.getGroundTruthGravityNorm());
28318         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28319         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28320         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28321                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28322         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28323         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28324         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28325 
28326         // Force IllegalArgumentException
28327         calibrator = null;
28328         try {
28329             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28330                     measurements, true, new double[1], this);
28331             fail("IllegalArgumentException expected but not thrown");
28332         } catch (final IllegalArgumentException ignore) {
28333         }
28334         assertNull(calibrator);
28335     }
28336 
28337     @Test
28338     public void testConstructor219() throws WrongSizeException {
28339         final Matrix ba = generateBa();
28340         final double[] bias = ba.getBuffer();
28341         final double biasX = ba.getElementAtIndex(0);
28342         final double biasY = ba.getElementAtIndex(1);
28343         final double biasZ = ba.getElementAtIndex(2);
28344 
28345         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28346         final double latitude = Math.toRadians(
28347                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
28348         final double longitude = Math.toRadians(
28349                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28350         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28351         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28352         final NEDVelocity nedVelocity = new NEDVelocity();
28353         final ECEFPosition ecefPosition = new ECEFPosition();
28354         final ECEFVelocity ecefVelocity = new ECEFVelocity();
28355         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28356                 ecefPosition, ecefVelocity);
28357 
28358         KnownPositionAccelerometerCalibrator calibrator =
28359                 new KnownPositionAccelerometerCalibrator(nedPosition, ba);
28360 
28361         // check default values
28362         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28363         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28364         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28365         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28366         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28367         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28368         final Acceleration bx2 = new Acceleration(0.0,
28369                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28370         calibrator.getInitialBiasXAsAcceleration(bx2);
28371         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28372         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28373         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28374         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28375         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28376         final Acceleration by2 = new Acceleration(0.0,
28377                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28378         calibrator.getInitialBiasYAsAcceleration(by2);
28379         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28380         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28381         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28382         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28383         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28384         final Acceleration bz2 = new Acceleration(0.0,
28385                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28386         calibrator.getInitialBiasZAsAcceleration(bz2);
28387         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28388         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28389         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28390         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28391         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28392         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28393         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
28394         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
28395         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
28396         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
28397         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
28398         final double[] bias1 = calibrator.getInitialBias();
28399         assertArrayEquals(bias1, bias, 0.0);
28400         final double[] bias2 = new double[3];
28401         calibrator.getInitialBias(bias2);
28402         assertArrayEquals(bias1, bias2, 0.0);
28403         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28404         assertEquals(b1, ba);
28405         final Matrix b2 = new Matrix(3, 1);
28406         calibrator.getInitialBiasAsMatrix(b2);
28407         assertEquals(b1, b2);
28408         final Matrix ma1 = calibrator.getInitialMa();
28409         assertEquals(ma1, new Matrix(3, 3));
28410         final Matrix ma2 = new Matrix(3, 3);
28411         calibrator.getInitialMa(ma2);
28412         assertEquals(ma1, ma2);
28413         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28414         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28415         final NEDPosition nedPosition1 = new NEDPosition();
28416         assertTrue(calibrator.getNedPosition(nedPosition1));
28417         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28418         assertNull(calibrator.getMeasurements());
28419         assertFalse(calibrator.isCommonAxisUsed());
28420         assertNull(calibrator.getListener());
28421         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
28422         assertFalse(calibrator.isReady());
28423         assertFalse(calibrator.isRunning());
28424         assertNull(calibrator.getEstimatedBiases());
28425         assertFalse(calibrator.getEstimatedBiases(null));
28426         assertNull(calibrator.getEstimatedBiasesAsMatrix());
28427         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28428         assertNull(calibrator.getEstimatedBiasFx());
28429         assertNull(calibrator.getEstimatedBiasFy());
28430         assertNull(calibrator.getEstimatedBiasFz());
28431         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28432         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28433         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28434         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28435         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28436         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28437         assertNull(calibrator.getEstimatedMa());
28438         assertNull(calibrator.getEstimatedSx());
28439         assertNull(calibrator.getEstimatedSy());
28440         assertNull(calibrator.getEstimatedSz());
28441         assertNull(calibrator.getEstimatedMxy());
28442         assertNull(calibrator.getEstimatedMxz());
28443         assertNull(calibrator.getEstimatedMyx());
28444         assertNull(calibrator.getEstimatedMyz());
28445         assertNull(calibrator.getEstimatedMzx());
28446         assertNull(calibrator.getEstimatedMzy());
28447         assertNull(calibrator.getEstimatedCovariance());
28448         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28449         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28450                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28451         assertNotNull(calibrator.getGroundTruthGravityNorm());
28452         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28453         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28454         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28455                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28456         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28457         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28458         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28459 
28460         // Force IllegalArgumentException
28461         calibrator = null;
28462         try {
28463             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28464                     new Matrix(1, 1));
28465             fail("IllegalArgumentException expected but not thrown");
28466         } catch (final IllegalArgumentException ignore) {
28467         }
28468         try {
28469             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28470                     new Matrix(1, 3));
28471             fail("IllegalArgumentException expected but not thrown");
28472         } catch (final IllegalArgumentException ignore) {
28473         }
28474         assertNull(calibrator);
28475     }
28476 
28477     @Test
28478     public void testConstructor220() throws WrongSizeException {
28479         final Matrix ba = generateBa();
28480         final double[] bias = ba.getBuffer();
28481         final double biasX = ba.getElementAtIndex(0);
28482         final double biasY = ba.getElementAtIndex(1);
28483         final double biasZ = ba.getElementAtIndex(2);
28484 
28485         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28486         final double latitude = Math.toRadians(
28487                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
28488         final double longitude = Math.toRadians(
28489                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28490         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28491         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28492         final NEDVelocity nedVelocity = new NEDVelocity();
28493         final ECEFPosition ecefPosition = new ECEFPosition();
28494         final ECEFVelocity ecefVelocity = new ECEFVelocity();
28495         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28496                 ecefPosition, ecefVelocity);
28497 
28498         KnownPositionAccelerometerCalibrator calibrator =
28499                 new KnownPositionAccelerometerCalibrator(nedPosition, ba,
28500                         this);
28501 
28502         // check default values
28503         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28504         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28505         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28506         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28507         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28508         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28509         final Acceleration bx2 = new Acceleration(0.0,
28510                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28511         calibrator.getInitialBiasXAsAcceleration(bx2);
28512         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28513         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28514         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28515         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28516         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28517         final Acceleration by2 = new Acceleration(0.0,
28518                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28519         calibrator.getInitialBiasYAsAcceleration(by2);
28520         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28521         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28522         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28523         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28524         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28525         final Acceleration bz2 = new Acceleration(0.0,
28526                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28527         calibrator.getInitialBiasZAsAcceleration(bz2);
28528         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28529         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28530         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28531         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28532         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28533         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28534         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
28535         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
28536         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
28537         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
28538         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
28539         final double[] bias1 = calibrator.getInitialBias();
28540         assertArrayEquals(bias1, bias, 0.0);
28541         final double[] bias2 = new double[3];
28542         calibrator.getInitialBias(bias2);
28543         assertArrayEquals(bias1, bias2, 0.0);
28544         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28545         assertEquals(b1, ba);
28546         final Matrix b2 = new Matrix(3, 1);
28547         calibrator.getInitialBiasAsMatrix(b2);
28548         assertEquals(b1, b2);
28549         final Matrix ma1 = calibrator.getInitialMa();
28550         assertEquals(ma1, new Matrix(3, 3));
28551         final Matrix ma2 = new Matrix(3, 3);
28552         calibrator.getInitialMa(ma2);
28553         assertEquals(ma1, ma2);
28554         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28555         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28556         final NEDPosition nedPosition1 = new NEDPosition();
28557         assertTrue(calibrator.getNedPosition(nedPosition1));
28558         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28559         assertNull(calibrator.getMeasurements());
28560         assertFalse(calibrator.isCommonAxisUsed());
28561         assertSame(calibrator.getListener(), this);
28562         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
28563         assertFalse(calibrator.isReady());
28564         assertFalse(calibrator.isRunning());
28565         assertNull(calibrator.getEstimatedBiases());
28566         assertFalse(calibrator.getEstimatedBiases(null));
28567         assertNull(calibrator.getEstimatedBiasesAsMatrix());
28568         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28569         assertNull(calibrator.getEstimatedBiasFx());
28570         assertNull(calibrator.getEstimatedBiasFy());
28571         assertNull(calibrator.getEstimatedBiasFz());
28572         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28573         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28574         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28575         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28576         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28577         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28578         assertNull(calibrator.getEstimatedMa());
28579         assertNull(calibrator.getEstimatedSx());
28580         assertNull(calibrator.getEstimatedSy());
28581         assertNull(calibrator.getEstimatedSz());
28582         assertNull(calibrator.getEstimatedMxy());
28583         assertNull(calibrator.getEstimatedMxz());
28584         assertNull(calibrator.getEstimatedMyx());
28585         assertNull(calibrator.getEstimatedMyz());
28586         assertNull(calibrator.getEstimatedMzx());
28587         assertNull(calibrator.getEstimatedMzy());
28588         assertNull(calibrator.getEstimatedCovariance());
28589         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28590         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28591                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28592         assertNotNull(calibrator.getGroundTruthGravityNorm());
28593         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28594         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28595         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28596                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28597         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28598         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28599         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28600 
28601         // Force IllegalArgumentException
28602         calibrator = null;
28603         try {
28604             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28605                     new Matrix(1, 1), this);
28606             fail("IllegalArgumentException expected but not thrown");
28607         } catch (final IllegalArgumentException ignore) {
28608         }
28609         try {
28610             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28611                     new Matrix(1, 3), this);
28612             fail("IllegalArgumentException expected but not thrown");
28613         } catch (final IllegalArgumentException ignore) {
28614         }
28615         assertNull(calibrator);
28616     }
28617 
28618     @Test
28619     public void testConstructor221() throws WrongSizeException {
28620         final Collection<StandardDeviationBodyKinematics> measurements =
28621                 Collections.emptyList();
28622 
28623         final Matrix ba = generateBa();
28624         final double[] bias = ba.getBuffer();
28625         final double biasX = ba.getElementAtIndex(0);
28626         final double biasY = ba.getElementAtIndex(1);
28627         final double biasZ = ba.getElementAtIndex(2);
28628 
28629         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28630         final double latitude = Math.toRadians(
28631                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
28632         final double longitude = Math.toRadians(
28633                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28634         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28635         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28636         final NEDVelocity nedVelocity = new NEDVelocity();
28637         final ECEFPosition ecefPosition = new ECEFPosition();
28638         final ECEFVelocity ecefVelocity = new ECEFVelocity();
28639         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28640                 ecefPosition, ecefVelocity);
28641 
28642         KnownPositionAccelerometerCalibrator calibrator =
28643                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
28644                         ba);
28645 
28646         // check default values
28647         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28648         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28649         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28650         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28651         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28652         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28653         final Acceleration bx2 = new Acceleration(0.0,
28654                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28655         calibrator.getInitialBiasXAsAcceleration(bx2);
28656         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28657         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28658         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28659         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28660         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28661         final Acceleration by2 = new Acceleration(0.0,
28662                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28663         calibrator.getInitialBiasYAsAcceleration(by2);
28664         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28665         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28666         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28667         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28668         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28669         final Acceleration bz2 = new Acceleration(0.0,
28670                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28671         calibrator.getInitialBiasZAsAcceleration(bz2);
28672         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28673         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28674         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28675         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28676         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28677         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28678         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
28679         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
28680         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
28681         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
28682         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
28683         final double[] bias1 = calibrator.getInitialBias();
28684         assertArrayEquals(bias1, bias, 0.0);
28685         final double[] bias2 = new double[3];
28686         calibrator.getInitialBias(bias2);
28687         assertArrayEquals(bias1, bias2, 0.0);
28688         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28689         assertEquals(b1, ba);
28690         final Matrix b2 = new Matrix(3, 1);
28691         calibrator.getInitialBiasAsMatrix(b2);
28692         assertEquals(b1, b2);
28693         final Matrix ma1 = calibrator.getInitialMa();
28694         assertEquals(ma1, new Matrix(3, 3));
28695         final Matrix ma2 = new Matrix(3, 3);
28696         calibrator.getInitialMa(ma2);
28697         assertEquals(ma1, ma2);
28698         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28699         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28700         final NEDPosition nedPosition1 = new NEDPosition();
28701         assertTrue(calibrator.getNedPosition(nedPosition1));
28702         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28703         assertSame(calibrator.getMeasurements(), measurements);
28704         assertFalse(calibrator.isCommonAxisUsed());
28705         assertNull(calibrator.getListener());
28706         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
28707         assertFalse(calibrator.isReady());
28708         assertFalse(calibrator.isRunning());
28709         assertNull(calibrator.getEstimatedBiases());
28710         assertFalse(calibrator.getEstimatedBiases(null));
28711         assertNull(calibrator.getEstimatedBiasesAsMatrix());
28712         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28713         assertNull(calibrator.getEstimatedBiasFx());
28714         assertNull(calibrator.getEstimatedBiasFy());
28715         assertNull(calibrator.getEstimatedBiasFz());
28716         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28717         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28718         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28719         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28720         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28721         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28722         assertNull(calibrator.getEstimatedMa());
28723         assertNull(calibrator.getEstimatedSx());
28724         assertNull(calibrator.getEstimatedSy());
28725         assertNull(calibrator.getEstimatedSz());
28726         assertNull(calibrator.getEstimatedMxy());
28727         assertNull(calibrator.getEstimatedMxz());
28728         assertNull(calibrator.getEstimatedMyx());
28729         assertNull(calibrator.getEstimatedMyz());
28730         assertNull(calibrator.getEstimatedMzx());
28731         assertNull(calibrator.getEstimatedMzy());
28732         assertNull(calibrator.getEstimatedCovariance());
28733         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28734         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28735                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28736         assertNotNull(calibrator.getGroundTruthGravityNorm());
28737         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28738         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28739         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28740                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28741         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28742         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28743         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28744 
28745         // Force IllegalArgumentException
28746         calibrator = null;
28747         try {
28748             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28749                     measurements, new Matrix(1, 1));
28750             fail("IllegalArgumentException expected but not thrown");
28751         } catch (final IllegalArgumentException ignore) {
28752         }
28753         try {
28754             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28755                     measurements, new Matrix(1, 3));
28756             fail("IllegalArgumentException expected but not thrown");
28757         } catch (final IllegalArgumentException ignore) {
28758         }
28759         assertNull(calibrator);
28760     }
28761 
28762     @Test
28763     public void testConstructor222() throws WrongSizeException {
28764         final Collection<StandardDeviationBodyKinematics> measurements =
28765                 Collections.emptyList();
28766 
28767         final Matrix ba = generateBa();
28768         final double[] bias = ba.getBuffer();
28769         final double biasX = ba.getElementAtIndex(0);
28770         final double biasY = ba.getElementAtIndex(1);
28771         final double biasZ = ba.getElementAtIndex(2);
28772 
28773         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28774         final double latitude = Math.toRadians(
28775                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
28776         final double longitude = Math.toRadians(
28777                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28778         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28779         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28780         final NEDVelocity nedVelocity = new NEDVelocity();
28781         final ECEFPosition ecefPosition = new ECEFPosition();
28782         final ECEFVelocity ecefVelocity = new ECEFVelocity();
28783         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28784                 ecefPosition, ecefVelocity);
28785 
28786         KnownPositionAccelerometerCalibrator calibrator =
28787                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
28788                         ba, this);
28789 
28790         // check default values
28791         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28792         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28793         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28794         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28795         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28796         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28797         final Acceleration bx2 = new Acceleration(0.0,
28798                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28799         calibrator.getInitialBiasXAsAcceleration(bx2);
28800         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28801         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28802         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28803         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28804         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28805         final Acceleration by2 = new Acceleration(0.0,
28806                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28807         calibrator.getInitialBiasYAsAcceleration(by2);
28808         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28809         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28810         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28811         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28812         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28813         final Acceleration bz2 = new Acceleration(0.0,
28814                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28815         calibrator.getInitialBiasZAsAcceleration(bz2);
28816         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28817         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28818         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28819         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28820         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28821         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28822         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
28823         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
28824         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
28825         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
28826         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
28827         final double[] bias1 = calibrator.getInitialBias();
28828         assertArrayEquals(bias1, bias, 0.0);
28829         final double[] bias2 = new double[3];
28830         calibrator.getInitialBias(bias2);
28831         assertArrayEquals(bias1, bias2, 0.0);
28832         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28833         assertEquals(b1, ba);
28834         final Matrix b2 = new Matrix(3, 1);
28835         calibrator.getInitialBiasAsMatrix(b2);
28836         assertEquals(b1, b2);
28837         final Matrix ma1 = calibrator.getInitialMa();
28838         assertEquals(ma1, new Matrix(3, 3));
28839         final Matrix ma2 = new Matrix(3, 3);
28840         calibrator.getInitialMa(ma2);
28841         assertEquals(ma1, ma2);
28842         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28843         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28844         final NEDPosition nedPosition1 = new NEDPosition();
28845         assertTrue(calibrator.getNedPosition(nedPosition1));
28846         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28847         assertSame(calibrator.getMeasurements(), measurements);
28848         assertFalse(calibrator.isCommonAxisUsed());
28849         assertSame(calibrator.getListener(), this);
28850         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
28851         assertFalse(calibrator.isReady());
28852         assertFalse(calibrator.isRunning());
28853         assertNull(calibrator.getEstimatedBiases());
28854         assertFalse(calibrator.getEstimatedBiases(null));
28855         assertNull(calibrator.getEstimatedBiasesAsMatrix());
28856         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28857         assertNull(calibrator.getEstimatedBiasFx());
28858         assertNull(calibrator.getEstimatedBiasFy());
28859         assertNull(calibrator.getEstimatedBiasFz());
28860         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28861         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28862         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28863         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28864         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28865         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28866         assertNull(calibrator.getEstimatedMa());
28867         assertNull(calibrator.getEstimatedSx());
28868         assertNull(calibrator.getEstimatedSy());
28869         assertNull(calibrator.getEstimatedSz());
28870         assertNull(calibrator.getEstimatedMxy());
28871         assertNull(calibrator.getEstimatedMxz());
28872         assertNull(calibrator.getEstimatedMyx());
28873         assertNull(calibrator.getEstimatedMyz());
28874         assertNull(calibrator.getEstimatedMzx());
28875         assertNull(calibrator.getEstimatedMzy());
28876         assertNull(calibrator.getEstimatedCovariance());
28877         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28878         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28879                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28880         assertNotNull(calibrator.getGroundTruthGravityNorm());
28881         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28882         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28883         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28884                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28885         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28886         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28887         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28888 
28889         // Force IllegalArgumentException
28890         calibrator = null;
28891         try {
28892             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28893                     measurements, new Matrix(1, 1), this);
28894             fail("IllegalArgumentException expected but not thrown");
28895         } catch (final IllegalArgumentException ignore) {
28896         }
28897         try {
28898             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28899                     measurements, new Matrix(1, 3), this);
28900             fail("IllegalArgumentException expected but not thrown");
28901         } catch (final IllegalArgumentException ignore) {
28902         }
28903         assertNull(calibrator);
28904     }
28905 
28906     @Test
28907     public void testConstructor223() throws WrongSizeException {
28908         final Matrix ba = generateBa();
28909         final double[] bias = ba.getBuffer();
28910         final double biasX = ba.getElementAtIndex(0);
28911         final double biasY = ba.getElementAtIndex(1);
28912         final double biasZ = ba.getElementAtIndex(2);
28913 
28914         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28915         final double latitude = Math.toRadians(
28916                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
28917         final double longitude = Math.toRadians(
28918                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28919         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28920         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28921         final NEDVelocity nedVelocity = new NEDVelocity();
28922         final ECEFPosition ecefPosition = new ECEFPosition();
28923         final ECEFVelocity ecefVelocity = new ECEFVelocity();
28924         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28925                 ecefPosition, ecefVelocity);
28926 
28927         KnownPositionAccelerometerCalibrator calibrator =
28928                 new KnownPositionAccelerometerCalibrator(nedPosition,
28929                         true, ba);
28930 
28931         // check default values
28932         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28933         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28934         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28935         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28936         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28937         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28938         final Acceleration bx2 = new Acceleration(0.0,
28939                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28940         calibrator.getInitialBiasXAsAcceleration(bx2);
28941         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28942         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28943         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28944         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28945         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28946         final Acceleration by2 = new Acceleration(0.0,
28947                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28948         calibrator.getInitialBiasYAsAcceleration(by2);
28949         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28950         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28951         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28952         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28953         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28954         final Acceleration bz2 = new Acceleration(0.0,
28955                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28956         calibrator.getInitialBiasZAsAcceleration(bz2);
28957         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28958         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28959         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28960         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28961         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28962         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28963         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
28964         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
28965         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
28966         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
28967         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
28968         final double[] bias1 = calibrator.getInitialBias();
28969         assertArrayEquals(bias1, bias, 0.0);
28970         final double[] bias2 = new double[3];
28971         calibrator.getInitialBias(bias2);
28972         assertArrayEquals(bias1, bias2, 0.0);
28973         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28974         assertEquals(b1, ba);
28975         final Matrix b2 = new Matrix(3, 1);
28976         calibrator.getInitialBiasAsMatrix(b2);
28977         assertEquals(b1, b2);
28978         final Matrix ma1 = calibrator.getInitialMa();
28979         assertEquals(ma1, new Matrix(3, 3));
28980         final Matrix ma2 = new Matrix(3, 3);
28981         calibrator.getInitialMa(ma2);
28982         assertEquals(ma1, ma2);
28983         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28984         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28985         final NEDPosition nedPosition1 = new NEDPosition();
28986         assertTrue(calibrator.getNedPosition(nedPosition1));
28987         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28988         assertNull(calibrator.getMeasurements());
28989         assertTrue(calibrator.isCommonAxisUsed());
28990         assertNull(calibrator.getListener());
28991         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
28992         assertFalse(calibrator.isReady());
28993         assertFalse(calibrator.isRunning());
28994         assertNull(calibrator.getEstimatedBiases());
28995         assertFalse(calibrator.getEstimatedBiases(null));
28996         assertNull(calibrator.getEstimatedBiasesAsMatrix());
28997         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28998         assertNull(calibrator.getEstimatedBiasFx());
28999         assertNull(calibrator.getEstimatedBiasFy());
29000         assertNull(calibrator.getEstimatedBiasFz());
29001         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
29002         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
29003         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
29004         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
29005         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
29006         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
29007         assertNull(calibrator.getEstimatedMa());
29008         assertNull(calibrator.getEstimatedSx());
29009         assertNull(calibrator.getEstimatedSy());
29010         assertNull(calibrator.getEstimatedSz());
29011         assertNull(calibrator.getEstimatedMxy());
29012         assertNull(calibrator.getEstimatedMxz());
29013         assertNull(calibrator.getEstimatedMyx());
29014         assertNull(calibrator.getEstimatedMyz());
29015         assertNull(calibrator.getEstimatedMzx());
29016         assertNull(calibrator.getEstimatedMzy());
29017         assertNull(calibrator.getEstimatedCovariance());
29018         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29019         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29020                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29021         assertNotNull(calibrator.getGroundTruthGravityNorm());
29022         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29023         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29024         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29025                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29026         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29027         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29028         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29029 
29030         // Force IllegalArgumentException
29031         calibrator = null;
29032         try {
29033             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29034                     true, new Matrix(1, 1));
29035             fail("IllegalArgumentException expected but not thrown");
29036         } catch (final IllegalArgumentException ignore) {
29037         }
29038         try {
29039             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29040                     true, new Matrix(1, 3));
29041             fail("IllegalArgumentException expected but not thrown");
29042         } catch (final IllegalArgumentException ignore) {
29043         }
29044         assertNull(calibrator);
29045     }
29046 
29047     @Test
29048     public void testConstructor224() throws WrongSizeException {
29049         final Matrix ba = generateBa();
29050         final double[] bias = ba.getBuffer();
29051         final double biasX = ba.getElementAtIndex(0);
29052         final double biasY = ba.getElementAtIndex(1);
29053         final double biasZ = ba.getElementAtIndex(2);
29054 
29055         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29056         final double latitude = Math.toRadians(
29057                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
29058         final double longitude = Math.toRadians(
29059                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29060         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29061         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29062         final NEDVelocity nedVelocity = new NEDVelocity();
29063         final ECEFPosition ecefPosition = new ECEFPosition();
29064         final ECEFVelocity ecefVelocity = new ECEFVelocity();
29065         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29066                 ecefPosition, ecefVelocity);
29067 
29068         KnownPositionAccelerometerCalibrator calibrator =
29069                 new KnownPositionAccelerometerCalibrator(nedPosition,
29070                         true, ba, this);
29071 
29072         // check default values
29073         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29074         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29075         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29076         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
29077         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
29078         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29079         final Acceleration bx2 = new Acceleration(0.0,
29080                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29081         calibrator.getInitialBiasXAsAcceleration(bx2);
29082         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
29083         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29084         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
29085         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
29086         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29087         final Acceleration by2 = new Acceleration(0.0,
29088                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29089         calibrator.getInitialBiasYAsAcceleration(by2);
29090         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
29091         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29092         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
29093         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
29094         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29095         final Acceleration bz2 = new Acceleration(0.0,
29096                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29097         calibrator.getInitialBiasZAsAcceleration(bz2);
29098         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
29099         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29100         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
29101         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
29102         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
29103         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
29104         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
29105         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
29106         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
29107         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
29108         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
29109         final double[] bias1 = calibrator.getInitialBias();
29110         assertArrayEquals(bias1, bias, 0.0);
29111         final double[] bias2 = new double[3];
29112         calibrator.getInitialBias(bias2);
29113         assertArrayEquals(bias1, bias2, 0.0);
29114         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
29115         assertEquals(b1, ba);
29116         final Matrix b2 = new Matrix(3, 1);
29117         calibrator.getInitialBiasAsMatrix(b2);
29118         assertEquals(b1, b2);
29119         final Matrix ma1 = calibrator.getInitialMa();
29120         assertEquals(ma1, new Matrix(3, 3));
29121         final Matrix ma2 = new Matrix(3, 3);
29122         calibrator.getInitialMa(ma2);
29123         assertEquals(ma1, ma2);
29124         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
29125         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
29126         final NEDPosition nedPosition1 = new NEDPosition();
29127         assertTrue(calibrator.getNedPosition(nedPosition1));
29128         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
29129         assertNull(calibrator.getMeasurements());
29130         assertTrue(calibrator.isCommonAxisUsed());
29131         assertSame(calibrator.getListener(), this);
29132         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
29133         assertFalse(calibrator.isReady());
29134         assertFalse(calibrator.isRunning());
29135         assertNull(calibrator.getEstimatedBiases());
29136         assertFalse(calibrator.getEstimatedBiases(null));
29137         assertNull(calibrator.getEstimatedBiasesAsMatrix());
29138         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
29139         assertNull(calibrator.getEstimatedBiasFx());
29140         assertNull(calibrator.getEstimatedBiasFy());
29141         assertNull(calibrator.getEstimatedBiasFz());
29142         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
29143         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
29144         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
29145         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
29146         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
29147         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
29148         assertNull(calibrator.getEstimatedMa());
29149         assertNull(calibrator.getEstimatedSx());
29150         assertNull(calibrator.getEstimatedSy());
29151         assertNull(calibrator.getEstimatedSz());
29152         assertNull(calibrator.getEstimatedMxy());
29153         assertNull(calibrator.getEstimatedMxz());
29154         assertNull(calibrator.getEstimatedMyx());
29155         assertNull(calibrator.getEstimatedMyz());
29156         assertNull(calibrator.getEstimatedMzx());
29157         assertNull(calibrator.getEstimatedMzy());
29158         assertNull(calibrator.getEstimatedCovariance());
29159         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29160         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29161                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29162         assertNotNull(calibrator.getGroundTruthGravityNorm());
29163         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29164         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29165         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29166                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29167         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29168         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29169         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29170 
29171         // Force IllegalArgumentException
29172         calibrator = null;
29173         try {
29174             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29175                     true, new Matrix(1, 1),
29176                     this);
29177             fail("IllegalArgumentException expected but not thrown");
29178         } catch (final IllegalArgumentException ignore) {
29179         }
29180         try {
29181             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29182                     true, new Matrix(1, 3),
29183                     this);
29184             fail("IllegalArgumentException expected but not thrown");
29185         } catch (final IllegalArgumentException ignore) {
29186         }
29187         assertNull(calibrator);
29188     }
29189 
29190     @Test
29191     public void testConstructor225() throws WrongSizeException {
29192         final Collection<StandardDeviationBodyKinematics> measurements =
29193                 Collections.emptyList();
29194 
29195         final Matrix ba = generateBa();
29196         final double[] bias = ba.getBuffer();
29197         final double biasX = ba.getElementAtIndex(0);
29198         final double biasY = ba.getElementAtIndex(1);
29199         final double biasZ = ba.getElementAtIndex(2);
29200 
29201         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29202         final double latitude = Math.toRadians(
29203                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
29204         final double longitude = Math.toRadians(
29205                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29206         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29207         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29208         final NEDVelocity nedVelocity = new NEDVelocity();
29209         final ECEFPosition ecefPosition = new ECEFPosition();
29210         final ECEFVelocity ecefVelocity = new ECEFVelocity();
29211         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29212                 ecefPosition, ecefVelocity);
29213 
29214         KnownPositionAccelerometerCalibrator calibrator =
29215                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
29216                         true, ba);
29217 
29218         // check default values
29219         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29220         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29221         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29222         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
29223         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
29224         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29225         final Acceleration bx2 = new Acceleration(0.0,
29226                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29227         calibrator.getInitialBiasXAsAcceleration(bx2);
29228         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
29229         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29230         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
29231         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
29232         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29233         final Acceleration by2 = new Acceleration(0.0,
29234                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29235         calibrator.getInitialBiasYAsAcceleration(by2);
29236         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
29237         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29238         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
29239         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
29240         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29241         final Acceleration bz2 = new Acceleration(0.0,
29242                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29243         calibrator.getInitialBiasZAsAcceleration(bz2);
29244         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
29245         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29246         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
29247         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
29248         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
29249         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
29250         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
29251         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
29252         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
29253         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
29254         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
29255         final double[] bias1 = calibrator.getInitialBias();
29256         assertArrayEquals(bias1, bias, 0.0);
29257         final double[] bias2 = new double[3];
29258         calibrator.getInitialBias(bias2);
29259         assertArrayEquals(bias1, bias2, 0.0);
29260         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
29261         assertEquals(b1, ba);
29262         final Matrix b2 = new Matrix(3, 1);
29263         calibrator.getInitialBiasAsMatrix(b2);
29264         assertEquals(b1, b2);
29265         final Matrix ma1 = calibrator.getInitialMa();
29266         assertEquals(ma1, new Matrix(3, 3));
29267         final Matrix ma2 = new Matrix(3, 3);
29268         calibrator.getInitialMa(ma2);
29269         assertEquals(ma1, ma2);
29270         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
29271         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
29272         final NEDPosition nedPosition1 = new NEDPosition();
29273         assertTrue(calibrator.getNedPosition(nedPosition1));
29274         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
29275         assertSame(calibrator.getMeasurements(), measurements);
29276         assertTrue(calibrator.isCommonAxisUsed());
29277         assertNull(calibrator.getListener());
29278         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
29279         assertFalse(calibrator.isReady());
29280         assertFalse(calibrator.isRunning());
29281         assertNull(calibrator.getEstimatedBiases());
29282         assertFalse(calibrator.getEstimatedBiases(null));
29283         assertNull(calibrator.getEstimatedBiasesAsMatrix());
29284         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
29285         assertNull(calibrator.getEstimatedBiasFx());
29286         assertNull(calibrator.getEstimatedBiasFy());
29287         assertNull(calibrator.getEstimatedBiasFz());
29288         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
29289         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
29290         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
29291         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
29292         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
29293         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
29294         assertNull(calibrator.getEstimatedMa());
29295         assertNull(calibrator.getEstimatedSx());
29296         assertNull(calibrator.getEstimatedSy());
29297         assertNull(calibrator.getEstimatedSz());
29298         assertNull(calibrator.getEstimatedMxy());
29299         assertNull(calibrator.getEstimatedMxz());
29300         assertNull(calibrator.getEstimatedMyx());
29301         assertNull(calibrator.getEstimatedMyz());
29302         assertNull(calibrator.getEstimatedMzx());
29303         assertNull(calibrator.getEstimatedMzy());
29304         assertNull(calibrator.getEstimatedCovariance());
29305         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29306         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29307                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29308         assertNotNull(calibrator.getGroundTruthGravityNorm());
29309         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29310         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29311         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29312                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29313         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29314         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29315         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29316 
29317         // Force IllegalArgumentException
29318         calibrator = null;
29319         try {
29320             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29321                     measurements, true,
29322                     new Matrix(1, 1));
29323             fail("IllegalArgumentException expected but not thrown");
29324         } catch (final IllegalArgumentException ignore) {
29325         }
29326         try {
29327             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29328                     measurements, true,
29329                     new Matrix(1, 3));
29330             fail("IllegalArgumentException expected but not thrown");
29331         } catch (final IllegalArgumentException ignore) {
29332         }
29333         assertNull(calibrator);
29334     }
29335 
29336     @Test
29337     public void testConstructor226() throws WrongSizeException {
29338         final Collection<StandardDeviationBodyKinematics> measurements =
29339                 Collections.emptyList();
29340 
29341         final Matrix ba = generateBa();
29342         final double[] bias = ba.getBuffer();
29343         final double biasX = ba.getElementAtIndex(0);
29344         final double biasY = ba.getElementAtIndex(1);
29345         final double biasZ = ba.getElementAtIndex(2);
29346 
29347         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29348         final double latitude = Math.toRadians(
29349                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
29350         final double longitude = Math.toRadians(
29351                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29352         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29353         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29354         final NEDVelocity nedVelocity = new NEDVelocity();
29355         final ECEFPosition ecefPosition = new ECEFPosition();
29356         final ECEFVelocity ecefVelocity = new ECEFVelocity();
29357         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29358                 ecefPosition, ecefVelocity);
29359 
29360         KnownPositionAccelerometerCalibrator calibrator =
29361                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
29362                         true, ba, this);
29363 
29364         // check default values
29365         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29366         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29367         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29368         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
29369         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
29370         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29371         final Acceleration bx2 = new Acceleration(0.0,
29372                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29373         calibrator.getInitialBiasXAsAcceleration(bx2);
29374         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
29375         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29376         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
29377         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
29378         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29379         final Acceleration by2 = new Acceleration(0.0,
29380                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29381         calibrator.getInitialBiasYAsAcceleration(by2);
29382         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
29383         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29384         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
29385         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
29386         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29387         final Acceleration bz2 = new Acceleration(0.0,
29388                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29389         calibrator.getInitialBiasZAsAcceleration(bz2);
29390         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
29391         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29392         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
29393         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
29394         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
29395         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
29396         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
29397         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
29398         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
29399         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
29400         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
29401         final double[] bias1 = calibrator.getInitialBias();
29402         assertArrayEquals(bias1, bias, 0.0);
29403         final double[] bias2 = new double[3];
29404         calibrator.getInitialBias(bias2);
29405         assertArrayEquals(bias1, bias2, 0.0);
29406         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
29407         assertEquals(b1, ba);
29408         final Matrix b2 = new Matrix(3, 1);
29409         calibrator.getInitialBiasAsMatrix(b2);
29410         assertEquals(b1, b2);
29411         final Matrix ma1 = calibrator.getInitialMa();
29412         assertEquals(ma1, new Matrix(3, 3));
29413         final Matrix ma2 = new Matrix(3, 3);
29414         calibrator.getInitialMa(ma2);
29415         assertEquals(ma1, ma2);
29416         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
29417         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
29418         final NEDPosition nedPosition1 = new NEDPosition();
29419         assertTrue(calibrator.getNedPosition(nedPosition1));
29420         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
29421         assertSame(calibrator.getMeasurements(), measurements);
29422         assertTrue(calibrator.isCommonAxisUsed());
29423         assertSame(calibrator.getListener(), this);
29424         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
29425         assertFalse(calibrator.isReady());
29426         assertFalse(calibrator.isRunning());
29427         assertNull(calibrator.getEstimatedBiases());
29428         assertFalse(calibrator.getEstimatedBiases(null));
29429         assertNull(calibrator.getEstimatedBiasesAsMatrix());
29430         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
29431         assertNull(calibrator.getEstimatedBiasFx());
29432         assertNull(calibrator.getEstimatedBiasFy());
29433         assertNull(calibrator.getEstimatedBiasFz());
29434         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
29435         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
29436         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
29437         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
29438         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
29439         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
29440         assertNull(calibrator.getEstimatedMa());
29441         assertNull(calibrator.getEstimatedSx());
29442         assertNull(calibrator.getEstimatedSy());
29443         assertNull(calibrator.getEstimatedSz());
29444         assertNull(calibrator.getEstimatedMxy());
29445         assertNull(calibrator.getEstimatedMxz());
29446         assertNull(calibrator.getEstimatedMyx());
29447         assertNull(calibrator.getEstimatedMyz());
29448         assertNull(calibrator.getEstimatedMzx());
29449         assertNull(calibrator.getEstimatedMzy());
29450         assertNull(calibrator.getEstimatedCovariance());
29451         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29452         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29453                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29454         assertNotNull(calibrator.getGroundTruthGravityNorm());
29455         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29456         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29457         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29458                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29459         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29460         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29461         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29462 
29463         // Force IllegalArgumentException
29464         calibrator = null;
29465         try {
29466             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29467                     measurements, true,
29468                     new Matrix(1, 1), this);
29469             fail("IllegalArgumentException expected but not thrown");
29470         } catch (final IllegalArgumentException ignore) {
29471         }
29472         try {
29473             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29474                     measurements, true,
29475                     new Matrix(1, 3), this);
29476             fail("IllegalArgumentException expected but not thrown");
29477         } catch (final IllegalArgumentException ignore) {
29478         }
29479         assertNull(calibrator);
29480     }
29481 
29482     @Test
29483     public void testConstructor227() throws WrongSizeException {
29484         final Matrix ba = generateBa();
29485         final double[] bias = ba.getBuffer();
29486         final double biasX = ba.getElementAtIndex(0);
29487         final double biasY = ba.getElementAtIndex(1);
29488         final double biasZ = ba.getElementAtIndex(2);
29489 
29490         final Matrix ma = generateMaCommonAxis();
29491         final double sx = ma.getElementAt(0, 0);
29492         final double sy = ma.getElementAt(1, 1);
29493         final double sz = ma.getElementAt(2, 2);
29494         final double mxy = ma.getElementAt(0, 1);
29495         final double mxz = ma.getElementAt(0, 2);
29496         final double myx = ma.getElementAt(1, 0);
29497         final double myz = ma.getElementAt(1, 2);
29498         final double mzx = ma.getElementAt(2, 0);
29499         final double mzy = ma.getElementAt(2, 1);
29500 
29501         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29502         final double latitude = Math.toRadians(
29503                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
29504         final double longitude = Math.toRadians(
29505                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29506         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29507         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29508         final NEDVelocity nedVelocity = new NEDVelocity();
29509         final ECEFPosition ecefPosition = new ECEFPosition();
29510         final ECEFVelocity ecefVelocity = new ECEFVelocity();
29511         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29512                 ecefPosition, ecefVelocity);
29513 
29514         KnownPositionAccelerometerCalibrator calibrator =
29515                 new KnownPositionAccelerometerCalibrator(nedPosition, ba, ma);
29516 
29517         // check default values
29518         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29519         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29520         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29521         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
29522         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
29523         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29524         final Acceleration bx2 = new Acceleration(0.0,
29525                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29526         calibrator.getInitialBiasXAsAcceleration(bx2);
29527         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
29528         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29529         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
29530         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
29531         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29532         final Acceleration by2 = new Acceleration(0.0,
29533                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29534         calibrator.getInitialBiasYAsAcceleration(by2);
29535         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
29536         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29537         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
29538         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
29539         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29540         final Acceleration bz2 = new Acceleration(0.0,
29541                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29542         calibrator.getInitialBiasZAsAcceleration(bz2);
29543         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
29544         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29545         assertEquals(calibrator.getInitialSx(), sx, 0.0);
29546         assertEquals(calibrator.getInitialSy(), sy, 0.0);
29547         assertEquals(calibrator.getInitialSz(), sz, 0.0);
29548         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
29549         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
29550         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
29551         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
29552         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
29553         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
29554         final double[] bias1 = calibrator.getInitialBias();
29555         assertArrayEquals(bias1, bias, 0.0);
29556         final double[] bias2 = new double[3];
29557         calibrator.getInitialBias(bias2);
29558         assertArrayEquals(bias1, bias2, 0.0);
29559         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
29560         assertEquals(b1, ba);
29561         final Matrix b2 = new Matrix(3, 1);
29562         calibrator.getInitialBiasAsMatrix(b2);
29563         assertEquals(b1, b2);
29564         final Matrix ma1 = new Matrix(3, 3);
29565         ma1.setSubmatrix(0, 0,
29566                 2, 2,
29567                 new double[]{sx, myx, mzx,
29568                         mxy, sy, mzy,
29569                         mxz, myz, sz});
29570         assertEquals(calibrator.getInitialMa(), ma1);
29571         final Matrix ma2 = new Matrix(3, 3);
29572         calibrator.getInitialMa(ma2);
29573         assertEquals(ma1, ma2);
29574         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
29575         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
29576         final NEDPosition nedPosition1 = new NEDPosition();
29577         assertTrue(calibrator.getNedPosition(nedPosition1));
29578         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
29579         assertNull(calibrator.getMeasurements());
29580         assertFalse(calibrator.isCommonAxisUsed());
29581         assertNull(calibrator.getListener());
29582         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
29583         assertFalse(calibrator.isReady());
29584         assertFalse(calibrator.isRunning());
29585         assertNull(calibrator.getEstimatedBiases());
29586         assertFalse(calibrator.getEstimatedBiases(null));
29587         assertNull(calibrator.getEstimatedBiasesAsMatrix());
29588         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
29589         assertNull(calibrator.getEstimatedBiasFx());
29590         assertNull(calibrator.getEstimatedBiasFy());
29591         assertNull(calibrator.getEstimatedBiasFz());
29592         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
29593         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
29594         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
29595         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
29596         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
29597         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
29598         assertNull(calibrator.getEstimatedMa());
29599         assertNull(calibrator.getEstimatedSx());
29600         assertNull(calibrator.getEstimatedSy());
29601         assertNull(calibrator.getEstimatedSz());
29602         assertNull(calibrator.getEstimatedMxy());
29603         assertNull(calibrator.getEstimatedMxz());
29604         assertNull(calibrator.getEstimatedMyx());
29605         assertNull(calibrator.getEstimatedMyz());
29606         assertNull(calibrator.getEstimatedMzx());
29607         assertNull(calibrator.getEstimatedMzy());
29608         assertNull(calibrator.getEstimatedCovariance());
29609         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29610         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29611                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29612         assertNotNull(calibrator.getGroundTruthGravityNorm());
29613         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29614         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29615         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29616                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29617         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29618         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29619         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29620 
29621         // Force IllegalArgumentException
29622         calibrator = null;
29623         try {
29624             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29625                     new Matrix(1, 1), ma);
29626             fail("IllegalArgumentException expected but not thrown");
29627         } catch (final IllegalArgumentException ignore) {
29628         }
29629         try {
29630             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29631                     new Matrix(1, 3), ma);
29632             fail("IllegalArgumentException expected but not thrown");
29633         } catch (final IllegalArgumentException ignore) {
29634         }
29635         try {
29636             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29637                     ba, new Matrix(1, 3));
29638             fail("IllegalArgumentException expected but not thrown");
29639         } catch (final IllegalArgumentException ignore) {
29640         }
29641         try {
29642             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29643                     ba, new Matrix(3, 1));
29644             fail("IllegalArgumentException expected but not thrown");
29645         } catch (final IllegalArgumentException ignore) {
29646         }
29647         assertNull(calibrator);
29648     }
29649 
29650     @Test
29651     public void testConstructor228() throws WrongSizeException {
29652         final Matrix ba = generateBa();
29653         final double[] bias = ba.getBuffer();
29654         final double biasX = ba.getElementAtIndex(0);
29655         final double biasY = ba.getElementAtIndex(1);
29656         final double biasZ = ba.getElementAtIndex(2);
29657 
29658         final Matrix ma = generateMaCommonAxis();
29659         final double sx = ma.getElementAt(0, 0);
29660         final double sy = ma.getElementAt(1, 1);
29661         final double sz = ma.getElementAt(2, 2);
29662         final double mxy = ma.getElementAt(0, 1);
29663         final double mxz = ma.getElementAt(0, 2);
29664         final double myx = ma.getElementAt(1, 0);
29665         final double myz = ma.getElementAt(1, 2);
29666         final double mzx = ma.getElementAt(2, 0);
29667         final double mzy = ma.getElementAt(2, 1);
29668 
29669         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29670         final double latitude = Math.toRadians(
29671                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
29672         final double longitude = Math.toRadians(
29673                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29674         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29675         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29676         final NEDVelocity nedVelocity = new NEDVelocity();
29677         final ECEFPosition ecefPosition = new ECEFPosition();
29678         final ECEFVelocity ecefVelocity = new ECEFVelocity();
29679         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29680                 ecefPosition, ecefVelocity);
29681 
29682         KnownPositionAccelerometerCalibrator calibrator =
29683                 new KnownPositionAccelerometerCalibrator(nedPosition,
29684                         ba, ma, this);
29685 
29686         // check default values
29687         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29688         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29689         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29690         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
29691         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
29692         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29693         final Acceleration bx2 = new Acceleration(0.0,
29694                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29695         calibrator.getInitialBiasXAsAcceleration(bx2);
29696         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
29697         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29698         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
29699         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
29700         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29701         final Acceleration by2 = new Acceleration(0.0,
29702                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29703         calibrator.getInitialBiasYAsAcceleration(by2);
29704         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
29705         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29706         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
29707         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
29708         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29709         final Acceleration bz2 = new Acceleration(0.0,
29710                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29711         calibrator.getInitialBiasZAsAcceleration(bz2);
29712         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
29713         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29714         assertEquals(calibrator.getInitialSx(), sx, 0.0);
29715         assertEquals(calibrator.getInitialSy(), sy, 0.0);
29716         assertEquals(calibrator.getInitialSz(), sz, 0.0);
29717         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
29718         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
29719         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
29720         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
29721         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
29722         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
29723         final double[] bias1 = calibrator.getInitialBias();
29724         assertArrayEquals(bias1, bias, 0.0);
29725         final double[] bias2 = new double[3];
29726         calibrator.getInitialBias(bias2);
29727         assertArrayEquals(bias1, bias2, 0.0);
29728         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
29729         assertEquals(b1, ba);
29730         final Matrix b2 = new Matrix(3, 1);
29731         calibrator.getInitialBiasAsMatrix(b2);
29732         assertEquals(b1, b2);
29733         final Matrix ma1 = new Matrix(3, 3);
29734         ma1.setSubmatrix(0, 0,
29735                 2, 2,
29736                 new double[]{sx, myx, mzx,
29737                         mxy, sy, mzy,
29738                         mxz, myz, sz});
29739         assertEquals(calibrator.getInitialMa(), ma1);
29740         final Matrix ma2 = new Matrix(3, 3);
29741         calibrator.getInitialMa(ma2);
29742         assertEquals(ma1, ma2);
29743         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
29744         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
29745         final NEDPosition nedPosition1 = new NEDPosition();
29746         assertTrue(calibrator.getNedPosition(nedPosition1));
29747         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
29748         assertNull(calibrator.getMeasurements());
29749         assertFalse(calibrator.isCommonAxisUsed());
29750         assertSame(calibrator.getListener(), this);
29751         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
29752         assertFalse(calibrator.isReady());
29753         assertFalse(calibrator.isRunning());
29754         assertNull(calibrator.getEstimatedBiases());
29755         assertFalse(calibrator.getEstimatedBiases(null));
29756         assertNull(calibrator.getEstimatedBiasesAsMatrix());
29757         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
29758         assertNull(calibrator.getEstimatedBiasFx());
29759         assertNull(calibrator.getEstimatedBiasFy());
29760         assertNull(calibrator.getEstimatedBiasFz());
29761         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
29762         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
29763         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
29764         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
29765         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
29766         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
29767         assertNull(calibrator.getEstimatedMa());
29768         assertNull(calibrator.getEstimatedSx());
29769         assertNull(calibrator.getEstimatedSy());
29770         assertNull(calibrator.getEstimatedSz());
29771         assertNull(calibrator.getEstimatedMxy());
29772         assertNull(calibrator.getEstimatedMxz());
29773         assertNull(calibrator.getEstimatedMyx());
29774         assertNull(calibrator.getEstimatedMyz());
29775         assertNull(calibrator.getEstimatedMzx());
29776         assertNull(calibrator.getEstimatedMzy());
29777         assertNull(calibrator.getEstimatedCovariance());
29778         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29779         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29780                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29781         assertNotNull(calibrator.getGroundTruthGravityNorm());
29782         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29783         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29784         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29785                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29786         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29787         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29788         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29789 
29790         // Force IllegalArgumentException
29791         calibrator = null;
29792         try {
29793             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29794                     new Matrix(1, 1), ma, this);
29795             fail("IllegalArgumentException expected but not thrown");
29796         } catch (final IllegalArgumentException ignore) {
29797         }
29798         try {
29799             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29800                     new Matrix(1, 3), ma, this);
29801             fail("IllegalArgumentException expected but not thrown");
29802         } catch (final IllegalArgumentException ignore) {
29803         }
29804         try {
29805             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29806                     ba, new Matrix(1, 3), this);
29807             fail("IllegalArgumentException expected but not thrown");
29808         } catch (final IllegalArgumentException ignore) {
29809         }
29810         try {
29811             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29812                     ba, new Matrix(3, 1), this);
29813             fail("IllegalArgumentException expected but not thrown");
29814         } catch (final IllegalArgumentException ignore) {
29815         }
29816         assertNull(calibrator);
29817     }
29818 
29819     @Test
29820     public void testConstructor229() throws WrongSizeException {
29821         final Collection<StandardDeviationBodyKinematics> measurements =
29822                 Collections.emptyList();
29823 
29824         final Matrix ba = generateBa();
29825         final double[] bias = ba.getBuffer();
29826         final double biasX = ba.getElementAtIndex(0);
29827         final double biasY = ba.getElementAtIndex(1);
29828         final double biasZ = ba.getElementAtIndex(2);
29829 
29830         final Matrix ma = generateMaCommonAxis();
29831         final double sx = ma.getElementAt(0, 0);
29832         final double sy = ma.getElementAt(1, 1);
29833         final double sz = ma.getElementAt(2, 2);
29834         final double mxy = ma.getElementAt(0, 1);
29835         final double mxz = ma.getElementAt(0, 2);
29836         final double myx = ma.getElementAt(1, 0);
29837         final double myz = ma.getElementAt(1, 2);
29838         final double mzx = ma.getElementAt(2, 0);
29839         final double mzy = ma.getElementAt(2, 1);
29840 
29841         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29842         final double latitude = Math.toRadians(
29843                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
29844         final double longitude = Math.toRadians(
29845                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29846         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29847         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29848         final NEDVelocity nedVelocity = new NEDVelocity();
29849         final ECEFPosition ecefPosition = new ECEFPosition();
29850         final ECEFVelocity ecefVelocity = new ECEFVelocity();
29851         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29852                 ecefPosition, ecefVelocity);
29853 
29854         KnownPositionAccelerometerCalibrator calibrator =
29855                 new KnownPositionAccelerometerCalibrator(nedPosition,
29856                         measurements, ba, ma);
29857 
29858         // check default values
29859         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29860         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29861         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29862         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
29863         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
29864         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29865         final Acceleration bx2 = new Acceleration(0.0,
29866                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29867         calibrator.getInitialBiasXAsAcceleration(bx2);
29868         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
29869         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29870         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
29871         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
29872         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29873         final Acceleration by2 = new Acceleration(0.0,
29874                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29875         calibrator.getInitialBiasYAsAcceleration(by2);
29876         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
29877         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29878         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
29879         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
29880         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29881         final Acceleration bz2 = new Acceleration(0.0,
29882                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29883         calibrator.getInitialBiasZAsAcceleration(bz2);
29884         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
29885         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29886         assertEquals(calibrator.getInitialSx(), sx, 0.0);
29887         assertEquals(calibrator.getInitialSy(), sy, 0.0);
29888         assertEquals(calibrator.getInitialSz(), sz, 0.0);
29889         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
29890         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
29891         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
29892         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
29893         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
29894         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
29895         final double[] bias1 = calibrator.getInitialBias();
29896         assertArrayEquals(bias1, bias, 0.0);
29897         final double[] bias2 = new double[3];
29898         calibrator.getInitialBias(bias2);
29899         assertArrayEquals(bias1, bias2, 0.0);
29900         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
29901         assertEquals(b1, ba);
29902         final Matrix b2 = new Matrix(3, 1);
29903         calibrator.getInitialBiasAsMatrix(b2);
29904         assertEquals(b1, b2);
29905         final Matrix ma1 = new Matrix(3, 3);
29906         ma1.setSubmatrix(0, 0,
29907                 2, 2,
29908                 new double[]{sx, myx, mzx,
29909                         mxy, sy, mzy,
29910                         mxz, myz, sz});
29911         assertEquals(calibrator.getInitialMa(), ma1);
29912         final Matrix ma2 = new Matrix(3, 3);
29913         calibrator.getInitialMa(ma2);
29914         assertEquals(ma1, ma2);
29915         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
29916         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
29917         final NEDPosition nedPosition1 = new NEDPosition();
29918         assertTrue(calibrator.getNedPosition(nedPosition1));
29919         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
29920         assertSame(calibrator.getMeasurements(), measurements);
29921         assertFalse(calibrator.isCommonAxisUsed());
29922         assertNull(calibrator.getListener());
29923         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
29924         assertFalse(calibrator.isReady());
29925         assertFalse(calibrator.isRunning());
29926         assertNull(calibrator.getEstimatedBiases());
29927         assertFalse(calibrator.getEstimatedBiases(null));
29928         assertNull(calibrator.getEstimatedBiasesAsMatrix());
29929         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
29930         assertNull(calibrator.getEstimatedBiasFx());
29931         assertNull(calibrator.getEstimatedBiasFy());
29932         assertNull(calibrator.getEstimatedBiasFz());
29933         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
29934         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
29935         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
29936         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
29937         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
29938         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
29939         assertNull(calibrator.getEstimatedMa());
29940         assertNull(calibrator.getEstimatedSx());
29941         assertNull(calibrator.getEstimatedSy());
29942         assertNull(calibrator.getEstimatedSz());
29943         assertNull(calibrator.getEstimatedMxy());
29944         assertNull(calibrator.getEstimatedMxz());
29945         assertNull(calibrator.getEstimatedMyx());
29946         assertNull(calibrator.getEstimatedMyz());
29947         assertNull(calibrator.getEstimatedMzx());
29948         assertNull(calibrator.getEstimatedMzy());
29949         assertNull(calibrator.getEstimatedCovariance());
29950         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29951         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29952                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29953         assertNotNull(calibrator.getGroundTruthGravityNorm());
29954         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29955         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29956         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29957                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29958         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29959         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29960         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29961 
29962         // Force IllegalArgumentException
29963         calibrator = null;
29964         try {
29965             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29966                     measurements, new Matrix(1, 1), ma);
29967             fail("IllegalArgumentException expected but not thrown");
29968         } catch (final IllegalArgumentException ignore) {
29969         }
29970         try {
29971             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29972                     measurements, new Matrix(1, 3), ma);
29973             fail("IllegalArgumentException expected but not thrown");
29974         } catch (final IllegalArgumentException ignore) {
29975         }
29976         try {
29977             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29978                     measurements, ba, new Matrix(1, 3));
29979             fail("IllegalArgumentException expected but not thrown");
29980         } catch (final IllegalArgumentException ignore) {
29981         }
29982         try {
29983             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29984                     measurements, ba, new Matrix(3, 1));
29985             fail("IllegalArgumentException expected but not thrown");
29986         } catch (final IllegalArgumentException ignore) {
29987         }
29988         assertNull(calibrator);
29989     }
29990 
29991     @Test
29992     public void testConstructor230() throws WrongSizeException {
29993         final Collection<StandardDeviationBodyKinematics> measurements =
29994                 Collections.emptyList();
29995 
29996         final Matrix ba = generateBa();
29997         final double[] bias = ba.getBuffer();
29998         final double biasX = ba.getElementAtIndex(0);
29999         final double biasY = ba.getElementAtIndex(1);
30000         final double biasZ = ba.getElementAtIndex(2);
30001 
30002         final Matrix ma = generateMaCommonAxis();
30003         final double sx = ma.getElementAt(0, 0);
30004         final double sy = ma.getElementAt(1, 1);
30005         final double sz = ma.getElementAt(2, 2);
30006         final double mxy = ma.getElementAt(0, 1);
30007         final double mxz = ma.getElementAt(0, 2);
30008         final double myx = ma.getElementAt(1, 0);
30009         final double myz = ma.getElementAt(1, 2);
30010         final double mzx = ma.getElementAt(2, 0);
30011         final double mzy = ma.getElementAt(2, 1);
30012 
30013         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
30014         final double latitude = Math.toRadians(
30015                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
30016         final double longitude = Math.toRadians(
30017                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
30018         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
30019         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
30020         final NEDVelocity nedVelocity = new NEDVelocity();
30021         final ECEFPosition ecefPosition = new ECEFPosition();
30022         final ECEFVelocity ecefVelocity = new ECEFVelocity();
30023         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
30024                 ecefPosition, ecefVelocity);
30025 
30026         KnownPositionAccelerometerCalibrator calibrator =
30027                 new KnownPositionAccelerometerCalibrator(nedPosition,
30028                         measurements, ba, ma, this);
30029 
30030         // check default values
30031         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
30032         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
30033         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
30034         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
30035         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
30036         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30037         final Acceleration bx2 = new Acceleration(0.0,
30038                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30039         calibrator.getInitialBiasXAsAcceleration(bx2);
30040         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
30041         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30042         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
30043         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
30044         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30045         final Acceleration by2 = new Acceleration(0.0,
30046                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30047         calibrator.getInitialBiasYAsAcceleration(by2);
30048         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
30049         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30050         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
30051         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
30052         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30053         final Acceleration bz2 = new Acceleration(0.0,
30054                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30055         calibrator.getInitialBiasZAsAcceleration(bz2);
30056         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
30057         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30058         assertEquals(calibrator.getInitialSx(), sx, 0.0);
30059         assertEquals(calibrator.getInitialSy(), sy, 0.0);
30060         assertEquals(calibrator.getInitialSz(), sz, 0.0);
30061         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
30062         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
30063         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
30064         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
30065         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
30066         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
30067         final double[] bias1 = calibrator.getInitialBias();
30068         assertArrayEquals(bias1, bias, 0.0);
30069         final double[] bias2 = new double[3];
30070         calibrator.getInitialBias(bias2);
30071         assertArrayEquals(bias1, bias2, 0.0);
30072         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
30073         assertEquals(b1, ba);
30074         final Matrix b2 = new Matrix(3, 1);
30075         calibrator.getInitialBiasAsMatrix(b2);
30076         assertEquals(b1, b2);
30077         final Matrix ma1 = new Matrix(3, 3);
30078         ma1.setSubmatrix(0, 0,
30079                 2, 2,
30080                 new double[]{sx, myx, mzx,
30081                         mxy, sy, mzy,
30082                         mxz, myz, sz});
30083         assertEquals(calibrator.getInitialMa(), ma1);
30084         final Matrix ma2 = new Matrix(3, 3);
30085         calibrator.getInitialMa(ma2);
30086         assertEquals(ma1, ma2);
30087         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
30088         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
30089         final NEDPosition nedPosition1 = new NEDPosition();
30090         assertTrue(calibrator.getNedPosition(nedPosition1));
30091         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
30092         assertSame(calibrator.getMeasurements(), measurements);
30093         assertFalse(calibrator.isCommonAxisUsed());
30094         assertSame(calibrator.getListener(), this);
30095         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
30096         assertFalse(calibrator.isReady());
30097         assertFalse(calibrator.isRunning());
30098         assertNull(calibrator.getEstimatedBiases());
30099         assertFalse(calibrator.getEstimatedBiases(null));
30100         assertNull(calibrator.getEstimatedBiasesAsMatrix());
30101         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
30102         assertNull(calibrator.getEstimatedBiasFx());
30103         assertNull(calibrator.getEstimatedBiasFy());
30104         assertNull(calibrator.getEstimatedBiasFz());
30105         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
30106         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
30107         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
30108         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
30109         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
30110         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
30111         assertNull(calibrator.getEstimatedMa());
30112         assertNull(calibrator.getEstimatedSx());
30113         assertNull(calibrator.getEstimatedSy());
30114         assertNull(calibrator.getEstimatedSz());
30115         assertNull(calibrator.getEstimatedMxy());
30116         assertNull(calibrator.getEstimatedMxz());
30117         assertNull(calibrator.getEstimatedMyx());
30118         assertNull(calibrator.getEstimatedMyz());
30119         assertNull(calibrator.getEstimatedMzx());
30120         assertNull(calibrator.getEstimatedMzy());
30121         assertNull(calibrator.getEstimatedCovariance());
30122         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
30123         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
30124                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
30125         assertNotNull(calibrator.getGroundTruthGravityNorm());
30126         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
30127         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
30128         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
30129                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
30130         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
30131         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
30132         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
30133 
30134         // Force IllegalArgumentException
30135         calibrator = null;
30136         try {
30137             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30138                     measurements, new Matrix(1, 1), ma, this);
30139             fail("IllegalArgumentException expected but not thrown");
30140         } catch (final IllegalArgumentException ignore) {
30141         }
30142         try {
30143             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30144                     measurements, new Matrix(1, 3), ma, this);
30145             fail("IllegalArgumentException expected but not thrown");
30146         } catch (final IllegalArgumentException ignore) {
30147         }
30148         try {
30149             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30150                     measurements, ba, new Matrix(1, 3), this);
30151             fail("IllegalArgumentException expected but not thrown");
30152         } catch (final IllegalArgumentException ignore) {
30153         }
30154         try {
30155             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30156                     measurements, ba, new Matrix(3, 1), this);
30157             fail("IllegalArgumentException expected but not thrown");
30158         } catch (final IllegalArgumentException ignore) {
30159         }
30160         assertNull(calibrator);
30161     }
30162 
30163     @Test
30164     public void testConstructor231() throws WrongSizeException {
30165         final Matrix ba = generateBa();
30166         final double[] bias = ba.getBuffer();
30167         final double biasX = ba.getElementAtIndex(0);
30168         final double biasY = ba.getElementAtIndex(1);
30169         final double biasZ = ba.getElementAtIndex(2);
30170 
30171         final Matrix ma = generateMaCommonAxis();
30172         final double sx = ma.getElementAt(0, 0);
30173         final double sy = ma.getElementAt(1, 1);
30174         final double sz = ma.getElementAt(2, 2);
30175         final double mxy = ma.getElementAt(0, 1);
30176         final double mxz = ma.getElementAt(0, 2);
30177         final double myx = ma.getElementAt(1, 0);
30178         final double myz = ma.getElementAt(1, 2);
30179         final double mzx = ma.getElementAt(2, 0);
30180         final double mzy = ma.getElementAt(2, 1);
30181 
30182         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
30183         final double latitude = Math.toRadians(
30184                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
30185         final double longitude = Math.toRadians(
30186                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
30187         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
30188         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
30189         final NEDVelocity nedVelocity = new NEDVelocity();
30190         final ECEFPosition ecefPosition = new ECEFPosition();
30191         final ECEFVelocity ecefVelocity = new ECEFVelocity();
30192         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
30193                 ecefPosition, ecefVelocity);
30194 
30195         KnownPositionAccelerometerCalibrator calibrator =
30196                 new KnownPositionAccelerometerCalibrator(nedPosition,
30197                         true, ba, ma);
30198 
30199         // check default values
30200         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
30201         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
30202         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
30203         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
30204         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
30205         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30206         final Acceleration bx2 = new Acceleration(0.0,
30207                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30208         calibrator.getInitialBiasXAsAcceleration(bx2);
30209         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
30210         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30211         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
30212         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
30213         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30214         final Acceleration by2 = new Acceleration(0.0,
30215                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30216         calibrator.getInitialBiasYAsAcceleration(by2);
30217         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
30218         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30219         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
30220         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
30221         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30222         final Acceleration bz2 = new Acceleration(0.0,
30223                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30224         calibrator.getInitialBiasZAsAcceleration(bz2);
30225         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
30226         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30227         assertEquals(calibrator.getInitialSx(), sx, 0.0);
30228         assertEquals(calibrator.getInitialSy(), sy, 0.0);
30229         assertEquals(calibrator.getInitialSz(), sz, 0.0);
30230         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
30231         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
30232         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
30233         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
30234         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
30235         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
30236         final double[] bias1 = calibrator.getInitialBias();
30237         assertArrayEquals(bias1, bias, 0.0);
30238         final double[] bias2 = new double[3];
30239         calibrator.getInitialBias(bias2);
30240         assertArrayEquals(bias1, bias2, 0.0);
30241         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
30242         assertEquals(b1, ba);
30243         final Matrix b2 = new Matrix(3, 1);
30244         calibrator.getInitialBiasAsMatrix(b2);
30245         assertEquals(b1, b2);
30246         final Matrix ma1 = new Matrix(3, 3);
30247         ma1.setSubmatrix(0, 0,
30248                 2, 2,
30249                 new double[]{sx, myx, mzx,
30250                         mxy, sy, mzy,
30251                         mxz, myz, sz});
30252         assertEquals(calibrator.getInitialMa(), ma1);
30253         final Matrix ma2 = new Matrix(3, 3);
30254         calibrator.getInitialMa(ma2);
30255         assertEquals(ma1, ma2);
30256         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
30257         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
30258         final NEDPosition nedPosition1 = new NEDPosition();
30259         assertTrue(calibrator.getNedPosition(nedPosition1));
30260         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
30261         assertNull(calibrator.getMeasurements());
30262         assertTrue(calibrator.isCommonAxisUsed());
30263         assertNull(calibrator.getListener());
30264         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
30265         assertFalse(calibrator.isReady());
30266         assertFalse(calibrator.isRunning());
30267         assertNull(calibrator.getEstimatedBiases());
30268         assertFalse(calibrator.getEstimatedBiases(null));
30269         assertNull(calibrator.getEstimatedBiasesAsMatrix());
30270         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
30271         assertNull(calibrator.getEstimatedBiasFx());
30272         assertNull(calibrator.getEstimatedBiasFy());
30273         assertNull(calibrator.getEstimatedBiasFz());
30274         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
30275         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
30276         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
30277         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
30278         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
30279         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
30280         assertNull(calibrator.getEstimatedMa());
30281         assertNull(calibrator.getEstimatedSx());
30282         assertNull(calibrator.getEstimatedSy());
30283         assertNull(calibrator.getEstimatedSz());
30284         assertNull(calibrator.getEstimatedMxy());
30285         assertNull(calibrator.getEstimatedMxz());
30286         assertNull(calibrator.getEstimatedMyx());
30287         assertNull(calibrator.getEstimatedMyz());
30288         assertNull(calibrator.getEstimatedMzx());
30289         assertNull(calibrator.getEstimatedMzy());
30290         assertNull(calibrator.getEstimatedCovariance());
30291         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
30292         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
30293                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
30294         assertNotNull(calibrator.getGroundTruthGravityNorm());
30295         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
30296         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
30297         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
30298                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
30299         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
30300         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
30301         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
30302 
30303         // Force IllegalArgumentException
30304         calibrator = null;
30305         try {
30306             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30307                     true, new Matrix(1, 1), ma);
30308             fail("IllegalArgumentException expected but not thrown");
30309         } catch (final IllegalArgumentException ignore) {
30310         }
30311         try {
30312             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30313                     true, new Matrix(1, 3), ma);
30314             fail("IllegalArgumentException expected but not thrown");
30315         } catch (final IllegalArgumentException ignore) {
30316         }
30317         try {
30318             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30319                     true, ba, new Matrix(1, 3));
30320             fail("IllegalArgumentException expected but not thrown");
30321         } catch (final IllegalArgumentException ignore) {
30322         }
30323         try {
30324             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30325                     true, ba, new Matrix(3, 1));
30326             fail("IllegalArgumentException expected but not thrown");
30327         } catch (final IllegalArgumentException ignore) {
30328         }
30329         assertNull(calibrator);
30330     }
30331 
30332     @Test
30333     public void testConstructor232() throws WrongSizeException {
30334         final Matrix ba = generateBa();
30335         final double[] bias = ba.getBuffer();
30336         final double biasX = ba.getElementAtIndex(0);
30337         final double biasY = ba.getElementAtIndex(1);
30338         final double biasZ = ba.getElementAtIndex(2);
30339 
30340         final Matrix ma = generateMaCommonAxis();
30341         final double sx = ma.getElementAt(0, 0);
30342         final double sy = ma.getElementAt(1, 1);
30343         final double sz = ma.getElementAt(2, 2);
30344         final double mxy = ma.getElementAt(0, 1);
30345         final double mxz = ma.getElementAt(0, 2);
30346         final double myx = ma.getElementAt(1, 0);
30347         final double myz = ma.getElementAt(1, 2);
30348         final double mzx = ma.getElementAt(2, 0);
30349         final double mzy = ma.getElementAt(2, 1);
30350 
30351         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
30352         final double latitude = Math.toRadians(
30353                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
30354         final double longitude = Math.toRadians(
30355                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
30356         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
30357         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
30358         final NEDVelocity nedVelocity = new NEDVelocity();
30359         final ECEFPosition ecefPosition = new ECEFPosition();
30360         final ECEFVelocity ecefVelocity = new ECEFVelocity();
30361         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
30362                 ecefPosition, ecefVelocity);
30363 
30364         KnownPositionAccelerometerCalibrator calibrator =
30365                 new KnownPositionAccelerometerCalibrator(nedPosition,
30366                         true, ba, ma, this);
30367 
30368         // check default values
30369         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
30370         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
30371         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
30372         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
30373         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
30374         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30375         final Acceleration bx2 = new Acceleration(0.0,
30376                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30377         calibrator.getInitialBiasXAsAcceleration(bx2);
30378         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
30379         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30380         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
30381         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
30382         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30383         final Acceleration by2 = new Acceleration(0.0,
30384                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30385         calibrator.getInitialBiasYAsAcceleration(by2);
30386         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
30387         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30388         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
30389         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
30390         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30391         final Acceleration bz2 = new Acceleration(0.0,
30392                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30393         calibrator.getInitialBiasZAsAcceleration(bz2);
30394         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
30395         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30396         assertEquals(calibrator.getInitialSx(), sx, 0.0);
30397         assertEquals(calibrator.getInitialSy(), sy, 0.0);
30398         assertEquals(calibrator.getInitialSz(), sz, 0.0);
30399         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
30400         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
30401         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
30402         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
30403         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
30404         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
30405         final double[] bias1 = calibrator.getInitialBias();
30406         assertArrayEquals(bias1, bias, 0.0);
30407         final double[] bias2 = new double[3];
30408         calibrator.getInitialBias(bias2);
30409         assertArrayEquals(bias1, bias2, 0.0);
30410         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
30411         assertEquals(b1, ba);
30412         final Matrix b2 = new Matrix(3, 1);
30413         calibrator.getInitialBiasAsMatrix(b2);
30414         assertEquals(b1, b2);
30415         final Matrix ma1 = new Matrix(3, 3);
30416         ma1.setSubmatrix(0, 0,
30417                 2, 2,
30418                 new double[]{sx, myx, mzx,
30419                         mxy, sy, mzy,
30420                         mxz, myz, sz});
30421         assertEquals(calibrator.getInitialMa(), ma1);
30422         final Matrix ma2 = new Matrix(3, 3);
30423         calibrator.getInitialMa(ma2);
30424         assertEquals(ma1, ma2);
30425         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
30426         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
30427         final NEDPosition nedPosition1 = new NEDPosition();
30428         assertTrue(calibrator.getNedPosition(nedPosition1));
30429         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
30430         assertNull(calibrator.getMeasurements());
30431         assertTrue(calibrator.isCommonAxisUsed());
30432         assertSame(calibrator.getListener(), this);
30433         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
30434         assertFalse(calibrator.isReady());
30435         assertFalse(calibrator.isRunning());
30436         assertNull(calibrator.getEstimatedBiases());
30437         assertFalse(calibrator.getEstimatedBiases(null));
30438         assertNull(calibrator.getEstimatedBiasesAsMatrix());
30439         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
30440         assertNull(calibrator.getEstimatedBiasFx());
30441         assertNull(calibrator.getEstimatedBiasFy());
30442         assertNull(calibrator.getEstimatedBiasFz());
30443         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
30444         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
30445         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
30446         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
30447         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
30448         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
30449         assertNull(calibrator.getEstimatedMa());
30450         assertNull(calibrator.getEstimatedSx());
30451         assertNull(calibrator.getEstimatedSy());
30452         assertNull(calibrator.getEstimatedSz());
30453         assertNull(calibrator.getEstimatedMxy());
30454         assertNull(calibrator.getEstimatedMxz());
30455         assertNull(calibrator.getEstimatedMyx());
30456         assertNull(calibrator.getEstimatedMyz());
30457         assertNull(calibrator.getEstimatedMzx());
30458         assertNull(calibrator.getEstimatedMzy());
30459         assertNull(calibrator.getEstimatedCovariance());
30460         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
30461         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
30462                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
30463         assertNotNull(calibrator.getGroundTruthGravityNorm());
30464         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
30465         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
30466         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
30467                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
30468         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
30469         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
30470         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
30471 
30472         // Force IllegalArgumentException
30473         calibrator = null;
30474         try {
30475             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30476                     true, new Matrix(1, 1), ma, this);
30477             fail("IllegalArgumentException expected but not thrown");
30478         } catch (final IllegalArgumentException ignore) {
30479         }
30480         try {
30481             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30482                     true, new Matrix(1, 3), ma, this);
30483             fail("IllegalArgumentException expected but not thrown");
30484         } catch (final IllegalArgumentException ignore) {
30485         }
30486         try {
30487             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30488                     true, ba, new Matrix(1, 3), this);
30489             fail("IllegalArgumentException expected but not thrown");
30490         } catch (final IllegalArgumentException ignore) {
30491         }
30492         try {
30493             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30494                     true, ba, new Matrix(3, 1), this);
30495             fail("IllegalArgumentException expected but not thrown");
30496         } catch (final IllegalArgumentException ignore) {
30497         }
30498         assertNull(calibrator);
30499     }
30500 
30501     @Test
30502     public void testConstructor233() throws WrongSizeException {
30503         final Collection<StandardDeviationBodyKinematics> measurements =
30504                 Collections.emptyList();
30505 
30506         final Matrix ba = generateBa();
30507         final double[] bias = ba.getBuffer();
30508         final double biasX = ba.getElementAtIndex(0);
30509         final double biasY = ba.getElementAtIndex(1);
30510         final double biasZ = ba.getElementAtIndex(2);
30511 
30512         final Matrix ma = generateMaCommonAxis();
30513         final double sx = ma.getElementAt(0, 0);
30514         final double sy = ma.getElementAt(1, 1);
30515         final double sz = ma.getElementAt(2, 2);
30516         final double mxy = ma.getElementAt(0, 1);
30517         final double mxz = ma.getElementAt(0, 2);
30518         final double myx = ma.getElementAt(1, 0);
30519         final double myz = ma.getElementAt(1, 2);
30520         final double mzx = ma.getElementAt(2, 0);
30521         final double mzy = ma.getElementAt(2, 1);
30522 
30523         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
30524         final double latitude = Math.toRadians(
30525                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
30526         final double longitude = Math.toRadians(
30527                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
30528         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
30529         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
30530         final NEDVelocity nedVelocity = new NEDVelocity();
30531         final ECEFPosition ecefPosition = new ECEFPosition();
30532         final ECEFVelocity ecefVelocity = new ECEFVelocity();
30533         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
30534                 ecefPosition, ecefVelocity);
30535 
30536         KnownPositionAccelerometerCalibrator calibrator =
30537                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
30538                         true, ba, ma);
30539 
30540         // check default values
30541         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
30542         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
30543         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
30544         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
30545         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
30546         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30547         final Acceleration bx2 = new Acceleration(0.0,
30548                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30549         calibrator.getInitialBiasXAsAcceleration(bx2);
30550         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
30551         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30552         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
30553         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
30554         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30555         final Acceleration by2 = new Acceleration(0.0,
30556                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30557         calibrator.getInitialBiasYAsAcceleration(by2);
30558         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
30559         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30560         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
30561         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
30562         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30563         final Acceleration bz2 = new Acceleration(0.0,
30564                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30565         calibrator.getInitialBiasZAsAcceleration(bz2);
30566         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
30567         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30568         assertEquals(calibrator.getInitialSx(), sx, 0.0);
30569         assertEquals(calibrator.getInitialSy(), sy, 0.0);
30570         assertEquals(calibrator.getInitialSz(), sz, 0.0);
30571         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
30572         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
30573         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
30574         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
30575         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
30576         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
30577         final double[] bias1 = calibrator.getInitialBias();
30578         assertArrayEquals(bias1, bias, 0.0);
30579         final double[] bias2 = new double[3];
30580         calibrator.getInitialBias(bias2);
30581         assertArrayEquals(bias1, bias2, 0.0);
30582         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
30583         assertEquals(b1, ba);
30584         final Matrix b2 = new Matrix(3, 1);
30585         calibrator.getInitialBiasAsMatrix(b2);
30586         assertEquals(b1, b2);
30587         final Matrix ma1 = new Matrix(3, 3);
30588         ma1.setSubmatrix(0, 0,
30589                 2, 2,
30590                 new double[]{sx, myx, mzx,
30591                         mxy, sy, mzy,
30592                         mxz, myz, sz});
30593         assertEquals(calibrator.getInitialMa(), ma1);
30594         final Matrix ma2 = new Matrix(3, 3);
30595         calibrator.getInitialMa(ma2);
30596         assertEquals(ma1, ma2);
30597         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
30598         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
30599         final NEDPosition nedPosition1 = new NEDPosition();
30600         assertTrue(calibrator.getNedPosition(nedPosition1));
30601         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
30602         assertSame(calibrator.getMeasurements(), measurements);
30603         assertTrue(calibrator.isCommonAxisUsed());
30604         assertNull(calibrator.getListener());
30605         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
30606         assertFalse(calibrator.isReady());
30607         assertFalse(calibrator.isRunning());
30608         assertNull(calibrator.getEstimatedBiases());
30609         assertFalse(calibrator.getEstimatedBiases(null));
30610         assertNull(calibrator.getEstimatedBiasesAsMatrix());
30611         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
30612         assertNull(calibrator.getEstimatedBiasFx());
30613         assertNull(calibrator.getEstimatedBiasFy());
30614         assertNull(calibrator.getEstimatedBiasFz());
30615         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
30616         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
30617         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
30618         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
30619         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
30620         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
30621         assertNull(calibrator.getEstimatedMa());
30622         assertNull(calibrator.getEstimatedSx());
30623         assertNull(calibrator.getEstimatedSy());
30624         assertNull(calibrator.getEstimatedSz());
30625         assertNull(calibrator.getEstimatedMxy());
30626         assertNull(calibrator.getEstimatedMxz());
30627         assertNull(calibrator.getEstimatedMyx());
30628         assertNull(calibrator.getEstimatedMyz());
30629         assertNull(calibrator.getEstimatedMzx());
30630         assertNull(calibrator.getEstimatedMzy());
30631         assertNull(calibrator.getEstimatedCovariance());
30632         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
30633         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
30634                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
30635         assertNotNull(calibrator.getGroundTruthGravityNorm());
30636         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
30637         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
30638         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
30639                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
30640         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
30641         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
30642         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
30643 
30644         // Force IllegalArgumentException
30645         calibrator = null;
30646         try {
30647             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30648                     measurements, true,
30649                     new Matrix(1, 1), ma);
30650             fail("IllegalArgumentException expected but not thrown");
30651         } catch (final IllegalArgumentException ignore) {
30652         }
30653         try {
30654             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30655                     measurements, true,
30656                     new Matrix(1, 3), ma);
30657             fail("IllegalArgumentException expected but not thrown");
30658         } catch (final IllegalArgumentException ignore) {
30659         }
30660         try {
30661             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30662                     measurements, true,
30663                     ba, new Matrix(1, 3));
30664             fail("IllegalArgumentException expected but not thrown");
30665         } catch (final IllegalArgumentException ignore) {
30666         }
30667         try {
30668             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30669                     measurements, true,
30670                     ba, new Matrix(3, 1));
30671             fail("IllegalArgumentException expected but not thrown");
30672         } catch (final IllegalArgumentException ignore) {
30673         }
30674         assertNull(calibrator);
30675     }
30676 
30677     @Test
30678     public void testConstructor234() throws WrongSizeException {
30679         final Collection<StandardDeviationBodyKinematics> measurements =
30680                 Collections.emptyList();
30681 
30682         final Matrix ba = generateBa();
30683         final double[] bias = ba.getBuffer();
30684         final double biasX = ba.getElementAtIndex(0);
30685         final double biasY = ba.getElementAtIndex(1);
30686         final double biasZ = ba.getElementAtIndex(2);
30687 
30688         final Matrix ma = generateMaCommonAxis();
30689         final double sx = ma.getElementAt(0, 0);
30690         final double sy = ma.getElementAt(1, 1);
30691         final double sz = ma.getElementAt(2, 2);
30692         final double mxy = ma.getElementAt(0, 1);
30693         final double mxz = ma.getElementAt(0, 2);
30694         final double myx = ma.getElementAt(1, 0);
30695         final double myz = ma.getElementAt(1, 2);
30696         final double mzx = ma.getElementAt(2, 0);
30697         final double mzy = ma.getElementAt(2, 1);
30698 
30699         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
30700         final double latitude = Math.toRadians(
30701                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
30702         final double longitude = Math.toRadians(
30703                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
30704         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
30705         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
30706         final NEDVelocity nedVelocity = new NEDVelocity();
30707         final ECEFPosition ecefPosition = new ECEFPosition();
30708         final ECEFVelocity ecefVelocity = new ECEFVelocity();
30709         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
30710                 ecefPosition, ecefVelocity);
30711 
30712         KnownPositionAccelerometerCalibrator calibrator =
30713                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
30714                         true, ba, ma, this);
30715 
30716         // check default values
30717         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
30718         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
30719         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
30720         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
30721         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
30722         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30723         final Acceleration bx2 = new Acceleration(0.0,
30724                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30725         calibrator.getInitialBiasXAsAcceleration(bx2);
30726         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
30727         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30728         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
30729         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
30730         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30731         final Acceleration by2 = new Acceleration(0.0,
30732                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30733         calibrator.getInitialBiasYAsAcceleration(by2);
30734         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
30735         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30736         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
30737         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
30738         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30739         final Acceleration bz2 = new Acceleration(0.0,
30740                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30741         calibrator.getInitialBiasZAsAcceleration(bz2);
30742         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
30743         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30744         assertEquals(calibrator.getInitialSx(), sx, 0.0);
30745         assertEquals(calibrator.getInitialSy(), sy, 0.0);
30746         assertEquals(calibrator.getInitialSz(), sz, 0.0);
30747         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
30748         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
30749         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
30750         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
30751         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
30752         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
30753         final double[] bias1 = calibrator.getInitialBias();
30754         assertArrayEquals(bias1, bias, 0.0);
30755         final double[] bias2 = new double[3];
30756         calibrator.getInitialBias(bias2);
30757         assertArrayEquals(bias1, bias2, 0.0);
30758         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
30759         assertEquals(b1, ba);
30760         final Matrix b2 = new Matrix(3, 1);
30761         calibrator.getInitialBiasAsMatrix(b2);
30762         assertEquals(b1, b2);
30763         final Matrix ma1 = new Matrix(3, 3);
30764         ma1.setSubmatrix(0, 0,
30765                 2, 2,
30766                 new double[]{sx, myx, mzx,
30767                         mxy, sy, mzy,
30768                         mxz, myz, sz});
30769         assertEquals(calibrator.getInitialMa(), ma1);
30770         final Matrix ma2 = new Matrix(3, 3);
30771         calibrator.getInitialMa(ma2);
30772         assertEquals(ma1, ma2);
30773         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
30774         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
30775         final NEDPosition nedPosition1 = new NEDPosition();
30776         assertTrue(calibrator.getNedPosition(nedPosition1));
30777         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
30778         assertSame(calibrator.getMeasurements(), measurements);
30779         assertTrue(calibrator.isCommonAxisUsed());
30780         assertSame(calibrator.getListener(), this);
30781         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
30782         assertFalse(calibrator.isReady());
30783         assertFalse(calibrator.isRunning());
30784         assertNull(calibrator.getEstimatedBiases());
30785         assertFalse(calibrator.getEstimatedBiases(null));
30786         assertNull(calibrator.getEstimatedBiasesAsMatrix());
30787         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
30788         assertNull(calibrator.getEstimatedBiasFx());
30789         assertNull(calibrator.getEstimatedBiasFy());
30790         assertNull(calibrator.getEstimatedBiasFz());
30791         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
30792         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
30793         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
30794         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
30795         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
30796         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
30797         assertNull(calibrator.getEstimatedMa());
30798         assertNull(calibrator.getEstimatedSx());
30799         assertNull(calibrator.getEstimatedSy());
30800         assertNull(calibrator.getEstimatedSz());
30801         assertNull(calibrator.getEstimatedMxy());
30802         assertNull(calibrator.getEstimatedMxz());
30803         assertNull(calibrator.getEstimatedMyx());
30804         assertNull(calibrator.getEstimatedMyz());
30805         assertNull(calibrator.getEstimatedMzx());
30806         assertNull(calibrator.getEstimatedMzy());
30807         assertNull(calibrator.getEstimatedCovariance());
30808         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
30809         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
30810                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
30811         assertNotNull(calibrator.getGroundTruthGravityNorm());
30812         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
30813         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
30814         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
30815                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
30816         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
30817         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
30818         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
30819 
30820         // Force IllegalArgumentException
30821         calibrator = null;
30822         try {
30823             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30824                     measurements, true,
30825                     new Matrix(1, 1), ma, this);
30826             fail("IllegalArgumentException expected but not thrown");
30827         } catch (final IllegalArgumentException ignore) {
30828         }
30829         try {
30830             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30831                     measurements, true,
30832                     new Matrix(1, 3), ma, this);
30833             fail("IllegalArgumentException expected but not thrown");
30834         } catch (final IllegalArgumentException ignore) {
30835         }
30836         try {
30837             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30838                     measurements, true,
30839                     ba, new Matrix(1, 3), this);
30840             fail("IllegalArgumentException expected but not thrown");
30841         } catch (final IllegalArgumentException ignore) {
30842         }
30843         try {
30844             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30845                     measurements, true,
30846                     ba, new Matrix(3, 1), this);
30847             fail("IllegalArgumentException expected but not thrown");
30848         } catch (final IllegalArgumentException ignore) {
30849         }
30850         assertNull(calibrator);
30851     }
30852 
30853     @Test
30854     public void testGetSetInitialBiasX() throws LockedException {
30855         final KnownPositionAccelerometerCalibrator calibrator =
30856                 new KnownPositionAccelerometerCalibrator();
30857 
30858         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
30859 
30860         // set new value
30861         final Matrix ba = generateBa();
30862         final double biasX = ba.getElementAtIndex(0);
30863 
30864         calibrator.setInitialBiasX(biasX);
30865 
30866         // check
30867         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
30868     }
30869 
30870     @Test
30871     public void testGetSetInitialBiasY() throws LockedException {
30872         final KnownPositionAccelerometerCalibrator calibrator =
30873                 new KnownPositionAccelerometerCalibrator();
30874 
30875         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
30876 
30877         // set new value
30878         final Matrix ba = generateBa();
30879         final double biasY = ba.getElementAtIndex(1);
30880 
30881         calibrator.setInitialBiasY(biasY);
30882 
30883         // check
30884         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
30885     }
30886 
30887     @Test
30888     public void testGetSetInitialBiasZ() throws LockedException {
30889         final KnownPositionAccelerometerCalibrator calibrator =
30890                 new KnownPositionAccelerometerCalibrator();
30891 
30892         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
30893 
30894         // set new value
30895         final Matrix ba = generateBa();
30896         final double biasZ = ba.getElementAtIndex(2);
30897 
30898         calibrator.setInitialBiasZ(biasZ);
30899 
30900         // check
30901         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
30902     }
30903 
30904     @Test
30905     public void testGetSetInitialBiasXAsAcceleration() throws LockedException {
30906         final KnownPositionAccelerometerCalibrator calibrator =
30907                 new KnownPositionAccelerometerCalibrator();
30908 
30909         // check default value
30910         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
30911 
30912         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
30913         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30914 
30915         // set new value
30916         final Matrix ba = generateBa();
30917         final double biasX = ba.getElementAtIndex(0);
30918 
30919         final Acceleration bx2 = new Acceleration(biasX,
30920                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
30921         calibrator.setInitialBiasX(bx2);
30922 
30923         // check
30924         final Acceleration bx3 = calibrator.getInitialBiasXAsAcceleration();
30925         final Acceleration bx4 = new Acceleration(biasX,
30926                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
30927         calibrator.getInitialBiasXAsAcceleration(bx4);
30928 
30929         assertEquals(bx3, bx4);
30930     }
30931 
30932     @Test
30933     public void testGetSetInitialBiasYAsAcceleration() throws LockedException {
30934         final KnownPositionAccelerometerCalibrator calibrator =
30935                 new KnownPositionAccelerometerCalibrator();
30936 
30937         // check default value
30938         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
30939 
30940         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
30941         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30942 
30943         // set new value
30944         final Matrix ba = generateBa();
30945         final double biasY = ba.getElementAtIndex(1);
30946 
30947         final Acceleration by2 = new Acceleration(biasY,
30948                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
30949         calibrator.setInitialBiasY(by2);
30950 
30951         // check
30952         final Acceleration by3 = calibrator.getInitialBiasYAsAcceleration();
30953         final Acceleration by4 = new Acceleration(biasY,
30954                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
30955         calibrator.getInitialBiasYAsAcceleration(by4);
30956 
30957         assertEquals(by3, by4);
30958     }
30959 
30960     @Test
30961     public void testGetSetInitialBiasZAsAcceleration() throws LockedException {
30962         final KnownPositionAccelerometerCalibrator calibrator =
30963                 new KnownPositionAccelerometerCalibrator();
30964 
30965         // check default value
30966         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
30967 
30968         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
30969         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30970 
30971         // set new value
30972         final Matrix ba = generateBa();
30973         final double biasZ = ba.getElementAtIndex(2);
30974 
30975         final Acceleration bz2 = new Acceleration(biasZ,
30976                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
30977         calibrator.setInitialBiasZ(bz2);
30978 
30979         // check
30980         final Acceleration bz3 = calibrator.getInitialBiasZAsAcceleration();
30981         final Acceleration bz4 = new Acceleration(biasZ,
30982                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
30983         calibrator.getInitialBiasZAsAcceleration(bz4);
30984 
30985         assertEquals(bz3, bz4);
30986     }
30987 
30988     @Test
30989     public void testSetInitialBias1() throws LockedException {
30990         final KnownPositionAccelerometerCalibrator calibrator =
30991                 new KnownPositionAccelerometerCalibrator();
30992 
30993         // check default values
30994         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
30995         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
30996         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
30997 
30998         // set new values
30999         final Matrix ba = generateBa();
31000         final double biasX = ba.getElementAtIndex(0);
31001         final double biasY = ba.getElementAtIndex(1);
31002         final double biasZ = ba.getElementAtIndex(2);
31003 
31004         calibrator.setInitialBias(biasX, biasY, biasZ);
31005 
31006         // check
31007         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
31008         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
31009         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
31010     }
31011 
31012     @Test
31013     public void testSetInitialBias2() throws LockedException {
31014         final KnownPositionAccelerometerCalibrator calibrator =
31015                 new KnownPositionAccelerometerCalibrator();
31016 
31017         // check default values
31018         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
31019         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
31020         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
31021 
31022         // set new values
31023         final Matrix ba = generateBa();
31024         final double biasX = ba.getElementAtIndex(0);
31025         final double biasY = ba.getElementAtIndex(1);
31026         final double biasZ = ba.getElementAtIndex(2);
31027 
31028         final Acceleration bx = new Acceleration(biasX,
31029                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
31030         final Acceleration by = new Acceleration(biasY,
31031                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
31032         final Acceleration bz = new Acceleration(biasZ,
31033                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
31034 
31035         calibrator.setInitialBias(bx, by, bz);
31036 
31037         // check
31038         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
31039         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
31040         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
31041     }
31042 
31043     @Test
31044     public void testGetSetInitialSx() throws WrongSizeException, LockedException {
31045         final KnownPositionAccelerometerCalibrator calibrator =
31046                 new KnownPositionAccelerometerCalibrator();
31047 
31048         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
31049 
31050         // set new value
31051         final Matrix ma = generateMaGeneral();
31052         final double sx = ma.getElementAt(0, 0);
31053 
31054         calibrator.setInitialSx(sx);
31055 
31056         // check
31057         assertEquals(calibrator.getInitialSx(), sx, 0.0);
31058     }
31059 
31060     @Test
31061     public void testGetSetInitialSy() throws WrongSizeException, LockedException {
31062         final KnownPositionAccelerometerCalibrator calibrator =
31063                 new KnownPositionAccelerometerCalibrator();
31064 
31065         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
31066 
31067         // set new value
31068         final Matrix ma = generateMaGeneral();
31069         final double sy = ma.getElementAt(1, 1);
31070 
31071         calibrator.setInitialSy(sy);
31072 
31073         // check
31074         assertEquals(calibrator.getInitialSy(), sy, 0.0);
31075     }
31076 
31077     @Test
31078     public void testGetSetInitialSz() throws WrongSizeException, LockedException {
31079         final KnownPositionAccelerometerCalibrator calibrator =
31080                 new KnownPositionAccelerometerCalibrator();
31081 
31082         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
31083 
31084         // set new value
31085         final Matrix ma = generateMaGeneral();
31086         final double sz = ma.getElementAt(2, 2);
31087 
31088         calibrator.setInitialSz(sz);
31089 
31090         // check
31091         assertEquals(calibrator.getInitialSz(), sz, 0.0);
31092     }
31093 
31094     @Test
31095     public void testGetSetInitialMxy() throws WrongSizeException, LockedException {
31096         final KnownPositionAccelerometerCalibrator calibrator =
31097                 new KnownPositionAccelerometerCalibrator();
31098 
31099         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
31100 
31101         // set new value
31102         final Matrix ma = generateMaGeneral();
31103         final double mxy = ma.getElementAt(0, 1);
31104 
31105         calibrator.setInitialMxy(mxy);
31106 
31107         // check
31108         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
31109     }
31110 
31111     @Test
31112     public void testGetSetInitialMxz() throws WrongSizeException, LockedException {
31113         final KnownPositionAccelerometerCalibrator calibrator =
31114                 new KnownPositionAccelerometerCalibrator();
31115 
31116         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
31117 
31118         // set new value
31119         final Matrix ma = generateMaGeneral();
31120         final double mxz = ma.getElementAt(0, 2);
31121 
31122         calibrator.setInitialMxz(mxz);
31123 
31124         // check
31125         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
31126     }
31127 
31128     @Test
31129     public void testGetSetInitialMyx() throws WrongSizeException, LockedException {
31130         final KnownPositionAccelerometerCalibrator calibrator =
31131                 new KnownPositionAccelerometerCalibrator();
31132 
31133         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
31134 
31135         // set new value
31136         final Matrix ma = generateMaGeneral();
31137         final double myx = ma.getElementAt(1, 0);
31138 
31139         calibrator.setInitialMyx(myx);
31140 
31141         // check
31142         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
31143     }
31144 
31145     @Test
31146     public void testGetSetInitialMyz() throws WrongSizeException, LockedException {
31147         final KnownPositionAccelerometerCalibrator calibrator =
31148                 new KnownPositionAccelerometerCalibrator();
31149 
31150         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
31151 
31152         // set new value
31153         final Matrix ma = generateMaGeneral();
31154         final double myz = ma.getElementAt(1, 2);
31155 
31156         calibrator.setInitialMyz(myz);
31157 
31158         // check
31159         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
31160     }
31161 
31162     @Test
31163     public void testGetSetInitialMzx() throws WrongSizeException, LockedException {
31164         final KnownPositionAccelerometerCalibrator calibrator =
31165                 new KnownPositionAccelerometerCalibrator();
31166 
31167         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
31168 
31169         // set new value
31170         final Matrix ma = generateMaGeneral();
31171         final double mzx = ma.getElementAt(2, 0);
31172 
31173         calibrator.setInitialMzx(mzx);
31174 
31175         // check
31176         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
31177     }
31178 
31179     @Test
31180     public void testGetSetInitialMzy() throws WrongSizeException, LockedException {
31181         final KnownPositionAccelerometerCalibrator calibrator =
31182                 new KnownPositionAccelerometerCalibrator();
31183 
31184         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
31185 
31186         // set new value
31187         final Matrix ma = generateMaGeneral();
31188         final double mzy = ma.getElementAt(2, 1);
31189 
31190         calibrator.setInitialMzy(mzy);
31191 
31192         // check
31193         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
31194     }
31195 
31196     @Test
31197     public void testSetInitialScalingFactors()
31198             throws WrongSizeException, LockedException {
31199 
31200         final KnownPositionAccelerometerCalibrator calibrator =
31201                 new KnownPositionAccelerometerCalibrator();
31202 
31203         // check default values
31204         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
31205         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
31206         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
31207 
31208         // set new values
31209         final Matrix ma = generateMaGeneral();
31210         final double sx = ma.getElementAt(0, 0);
31211         final double sy = ma.getElementAt(1, 1);
31212         final double sz = ma.getElementAt(2, 2);
31213 
31214         calibrator.setInitialScalingFactors(sx, sy, sz);
31215 
31216         // check
31217         assertEquals(calibrator.getInitialSx(), sx, 0.0);
31218         assertEquals(calibrator.getInitialSy(), sy, 0.0);
31219         assertEquals(calibrator.getInitialSz(), sz, 0.0);
31220     }
31221 
31222     @Test
31223     public void testSetInitialCrossCouplingErrors()
31224             throws WrongSizeException, LockedException {
31225 
31226         final KnownPositionAccelerometerCalibrator calibrator =
31227                 new KnownPositionAccelerometerCalibrator();
31228 
31229         // check default values
31230         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
31231         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
31232         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
31233         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
31234         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
31235         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
31236 
31237         // set new values
31238         final Matrix ma = generateMaGeneral();
31239         final double mxy = ma.getElementAt(0, 1);
31240         final double mxz = ma.getElementAt(0, 2);
31241         final double myx = ma.getElementAt(1, 0);
31242         final double myz = ma.getElementAt(1, 2);
31243         final double mzx = ma.getElementAt(2, 0);
31244         final double mzy = ma.getElementAt(2, 1);
31245 
31246         calibrator.setInitialCrossCouplingErrors(mxy, mxz, myx, myz, mzx, mzy);
31247 
31248         // check
31249         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
31250         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
31251         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
31252         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
31253         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
31254         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
31255     }
31256 
31257     @Test
31258     public void testSetInitialScalingFactorsAndCrossCouplingErrors()
31259             throws WrongSizeException, LockedException {
31260 
31261         final KnownPositionAccelerometerCalibrator calibrator =
31262                 new KnownPositionAccelerometerCalibrator();
31263 
31264         // check default values
31265         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
31266         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
31267         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
31268         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
31269         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
31270         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
31271         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
31272         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
31273         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
31274 
31275         // set new values
31276         final Matrix ma = generateMaGeneral();
31277         final double sx = ma.getElementAt(0, 0);
31278         final double sy = ma.getElementAt(1, 1);
31279         final double sz = ma.getElementAt(2, 2);
31280         final double mxy = ma.getElementAt(0, 1);
31281         final double mxz = ma.getElementAt(0, 2);
31282         final double myx = ma.getElementAt(1, 0);
31283         final double myz = ma.getElementAt(1, 2);
31284         final double mzx = ma.getElementAt(2, 0);
31285         final double mzy = ma.getElementAt(2, 1);
31286 
31287         calibrator.setInitialScalingFactorsAndCrossCouplingErrors(sx, sy, sz,
31288                 mxy, mxz, myx, myz, mzx, mzy);
31289 
31290         // check
31291         assertEquals(calibrator.getInitialSx(), sx, 0.0);
31292         assertEquals(calibrator.getInitialSy(), sy, 0.0);
31293         assertEquals(calibrator.getInitialSz(), sz, 0.0);
31294         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
31295         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
31296         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
31297         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
31298         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
31299         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
31300     }
31301 
31302     @Test
31303     public void testGetSetInitialBias() throws LockedException {
31304         final KnownPositionAccelerometerCalibrator calibrator =
31305                 new KnownPositionAccelerometerCalibrator();
31306 
31307         // check default value
31308         final double[] bias1 = calibrator.getInitialBias();
31309         assertArrayEquals(bias1, new double[3], 0.0);
31310 
31311         // set new values
31312         final Matrix ba = generateBa();
31313         final double[] bias2 = ba.getBuffer();
31314         calibrator.setInitialBias(bias2);
31315 
31316         // check
31317         final double[] bias3 = calibrator.getInitialBias();
31318         final double[] bias4 = new double[3];
31319         calibrator.getInitialBias(bias4);
31320 
31321         assertArrayEquals(bias2, bias3, 0.0);
31322         assertArrayEquals(bias2, bias4, 0.0);
31323 
31324         // Force IllegalArgumentException
31325         try {
31326             calibrator.getInitialBias(new double[1]);
31327             fail("IllegalArgumentException expected but not thrown");
31328         } catch (final IllegalArgumentException ignore) {
31329         }
31330         try {
31331             calibrator.setInitialBias(new double[1]);
31332             fail("IllegalArgumentException expected but not thrown");
31333         } catch (final IllegalArgumentException ignore) {
31334         }
31335     }
31336 
31337     @Test
31338     public void testGetSetInitialBiasAsMatrix() throws WrongSizeException,
31339             LockedException {
31340 
31341         final KnownPositionAccelerometerCalibrator calibrator =
31342                 new KnownPositionAccelerometerCalibrator();
31343 
31344         // check default value
31345         final Matrix bias1 = calibrator.getInitialBiasAsMatrix();
31346         assertEquals(bias1, new Matrix(3, 1));
31347 
31348         // set new value
31349         final Matrix bias2 = generateBa();
31350         calibrator.setInitialBias(bias2);
31351 
31352         // check
31353         final Matrix bias3 = calibrator.getInitialBiasAsMatrix();
31354         final Matrix bias4 = new Matrix(3, 1);
31355         calibrator.getInitialBiasAsMatrix(bias4);
31356 
31357         assertEquals(bias2, bias3);
31358         assertEquals(bias2, bias4);
31359 
31360         // Force IllegalArgumentException
31361         try {
31362             calibrator.getInitialBiasAsMatrix(new Matrix(1, 1));
31363             fail("IllegalArgumentException expected but not thrown");
31364         } catch (final IllegalArgumentException ignore) {
31365         }
31366         try {
31367             calibrator.getInitialBiasAsMatrix(new Matrix(3, 3));
31368             fail("IllegalArgumentException expected but not thrown");
31369         } catch (final IllegalArgumentException ignore) {
31370         }
31371         try {
31372             calibrator.setInitialBias(new Matrix(1, 1));
31373             fail("IllegalArgumentException expected but not thrown");
31374         } catch (final IllegalArgumentException ignore) {
31375         }
31376         try {
31377             calibrator.setInitialBias(new Matrix(3, 3));
31378             fail("IllegalArgumentException expected but not thrown");
31379         } catch (final IllegalArgumentException ignore) {
31380         }
31381     }
31382 
31383     @Test
31384     public void testGetSetInitialMa() throws WrongSizeException, LockedException {
31385         final KnownPositionAccelerometerCalibrator calibrator =
31386                 new KnownPositionAccelerometerCalibrator();
31387 
31388         // test default value
31389         final Matrix ma1 = calibrator.getInitialMa();
31390         assertEquals(ma1, new Matrix(3, 3));
31391 
31392         // set new value
31393         final Matrix ma2 = generateMaGeneral();
31394         calibrator.setInitialMa(ma2);
31395 
31396         // check
31397         final Matrix ma3 = calibrator.getInitialMa();
31398         final Matrix ma4 = new Matrix(3, 3);
31399         calibrator.getInitialMa(ma4);
31400 
31401         assertEquals(ma2, ma3);
31402         assertEquals(ma2, ma4);
31403 
31404         // Force IllegalArgumentException
31405         try {
31406             calibrator.getInitialMa(new Matrix(1, 3));
31407             fail("IllegalArgumentException expected but not thrown");
31408         } catch (final IllegalArgumentException ignore) {
31409         }
31410         try {
31411             calibrator.getInitialMa(new Matrix(3, 1));
31412             fail("IllegalArgumentException expected but not thrown");
31413         } catch (final IllegalArgumentException ignore) {
31414         }
31415         try {
31416             calibrator.setInitialMa(new Matrix(1, 3));
31417             fail("IllegalArgumentException expected but not thrown");
31418         } catch (final IllegalArgumentException ignore) {
31419         }
31420         try {
31421             calibrator.setInitialMa(new Matrix(3, 1));
31422             fail("IllegalArgumentException expected but not thrown");
31423         } catch (final IllegalArgumentException ignore) {
31424         }
31425     }
31426 
31427     @Test
31428     public void testGetSetEcefPosition() throws LockedException {
31429         final KnownPositionAccelerometerCalibrator calibrator =
31430                 new KnownPositionAccelerometerCalibrator();
31431 
31432         // check default value
31433         assertNull(calibrator.getEcefPosition());
31434 
31435         // set new value
31436         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
31437         final double latitude = Math.toRadians(
31438                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
31439         final double longitude = Math.toRadians(
31440                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
31441         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
31442         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
31443         final NEDVelocity nedVelocity = new NEDVelocity();
31444         final ECEFPosition ecefPosition = new ECEFPosition();
31445         final ECEFVelocity ecefVelocity = new ECEFVelocity();
31446         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
31447                 ecefPosition, ecefVelocity);
31448 
31449         calibrator.setPosition(ecefPosition);
31450 
31451         // check
31452         assertSame(calibrator.getEcefPosition(), ecefPosition);
31453     }
31454 
31455     @Test
31456     public void testGetSetNedPosition() throws LockedException {
31457         final KnownPositionAccelerometerCalibrator calibrator =
31458                 new KnownPositionAccelerometerCalibrator();
31459 
31460         // check default value
31461         assertNull(calibrator.getNedPosition());
31462         assertFalse(calibrator.getNedPosition(null));
31463 
31464         // set new value
31465         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
31466         final double latitude = Math.toRadians(
31467                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
31468         final double longitude = Math.toRadians(
31469                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
31470         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
31471         final NEDPosition nedPosition1 = new NEDPosition(latitude, longitude, height);
31472 
31473         calibrator.setPosition(nedPosition1);
31474 
31475         // check
31476         final NEDPosition nedPosition2 = calibrator.getNedPosition();
31477         final NEDPosition nedPosition3 = new NEDPosition();
31478         calibrator.getNedPosition(nedPosition3);
31479 
31480         assertTrue(nedPosition1.equals(nedPosition2, ABSOLUTE_ERROR));
31481         assertTrue(nedPosition1.equals(nedPosition3, ABSOLUTE_ERROR));
31482     }
31483 
31484     @Test
31485     public void testGetSetMeasurements() throws LockedException {
31486         final KnownPositionAccelerometerCalibrator calibrator =
31487                 new KnownPositionAccelerometerCalibrator();
31488 
31489         // check default value
31490         assertNull(calibrator.getMeasurements());
31491 
31492         // set new value
31493         final Collection<StandardDeviationBodyKinematics> measurements =
31494                 Collections.emptyList();
31495         calibrator.setMeasurements(measurements);
31496 
31497         // check
31498         assertSame(calibrator.getMeasurements(), measurements);
31499     }
31500 
31501     @Test
31502     public void testIsSetCommonAxisUsed() throws LockedException {
31503         final KnownPositionAccelerometerCalibrator calibrator =
31504                 new KnownPositionAccelerometerCalibrator();
31505 
31506         // check default value
31507         assertFalse(calibrator.isCommonAxisUsed());
31508 
31509         // set new value
31510         calibrator.setCommonAxisUsed(true);
31511 
31512         // check
31513         assertTrue(calibrator.isCommonAxisUsed());
31514     }
31515 
31516     @Test
31517     public void testGetSetListener() throws LockedException {
31518         final KnownPositionAccelerometerCalibrator calibrator =
31519                 new KnownPositionAccelerometerCalibrator();
31520 
31521         // check default value
31522         assertNull(calibrator.getListener());
31523 
31524         // set new value
31525         calibrator.setListener(this);
31526 
31527         // check
31528         assertSame(calibrator.getListener(), this);
31529     }
31530 
31531     @Test
31532     public void testIsReady() throws LockedException {
31533         final KnownPositionAccelerometerCalibrator calibrator =
31534                 new KnownPositionAccelerometerCalibrator();
31535 
31536         // check
31537         assertFalse(calibrator.isReady());
31538 
31539 
31540         // set empty measurements
31541         final Collection<StandardDeviationBodyKinematics> measurements1 =
31542                 Collections.emptyList();
31543         calibrator.setMeasurements(measurements1);
31544 
31545         // check
31546         assertFalse(calibrator.isReady());
31547 
31548 
31549         // set enough measurements for general case
31550         calibrator.setCommonAxisUsed(false);
31551 
31552         final List<StandardDeviationBodyKinematics> measurements2 = new ArrayList<>();
31553         for (int i = 0; i < KnownPositionAccelerometerCalibrator.MINIMUM_MEASUREMENTS_GENERAL; i++) {
31554             measurements2.add(new StandardDeviationBodyKinematics());
31555         }
31556         calibrator.setMeasurements(measurements2);
31557 
31558         // check
31559         assertFalse(calibrator.isReady());
31560 
31561 
31562         // set position
31563         final ECEFPosition position = new ECEFPosition();
31564         calibrator.setPosition(position);
31565 
31566         assertTrue(calibrator.isReady());
31567 
31568 
31569         // set enough measurements for common axis case
31570         measurements2.clear();
31571         for (int i = 0; i < KnownPositionAccelerometerCalibrator.MINIMUM_MEASUREMENTS_COMON_Z_AXIS; i++) {
31572             measurements2.add(new StandardDeviationBodyKinematics());
31573         }
31574         calibrator.setMeasurements(measurements2);
31575 
31576         // check
31577         assertFalse(calibrator.isReady());
31578 
31579         calibrator.setCommonAxisUsed(true);
31580 
31581         assertTrue(calibrator.isReady());
31582     }
31583 
31584     @Test
31585     public void testCalibrateForGeneralCaseWithMinimumMeasuresAndNoNoise()
31586             throws WrongSizeException, InvalidSourceAndDestinationFrameTypeException,
31587             LockedException, NotReadyException {
31588 
31589         int numValid = 0;
31590         for (int t = 0; t < TIMES; t++) {
31591             final Matrix ba = generateBa();
31592             final Matrix bg = generateBg();
31593             final Matrix ma = generateMaGeneral();
31594             final Matrix mg = generateMg();
31595             final Matrix gg = generateGg();
31596             // when using minimum number of measurements we must not add any noise so that
31597             // a solution is found, when adding more measurements, certain noise can be added
31598             final double accelNoiseRootPSD = 0.0;
31599             final double gyroNoiseRootPSD = 0.0;
31600             final double accelQuantLevel = 0.0;
31601             final double gyroQuantLevel = 0.0;
31602 
31603             final IMUErrors errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD,
31604                     gyroNoiseRootPSD, accelQuantLevel, gyroQuantLevel);
31605 
31606             final Random random = new Random();
31607             final UniformRandomizer randomizer = new UniformRandomizer(random);
31608             final double latitude = Math.toRadians(
31609                     randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
31610             final double longitude = Math.toRadians(
31611                     randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
31612             final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
31613             final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
31614 
31615             final double sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
31616             final double specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
31617             final double angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
31618 
31619             final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();
31620             for (int i = 0; i < KnownPositionAccelerometerCalibrator.MINIMUM_MEASUREMENTS_GENERAL; i++) {
31621                 final double roll = Math.toRadians(
31622                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31623                 final double pitch = Math.toRadians(
31624                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31625                 final double yaw = Math.toRadians(
31626                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31627                 final CoordinateTransformation nedC = new CoordinateTransformation(
31628                         roll, pitch, yaw, FrameType.BODY_FRAME,
31629                         FrameType.LOCAL_NAVIGATION_FRAME);
31630 
31631                 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
31632                 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
31633                         .convertNEDtoECEFAndReturnNew(nedFrame);
31634 
31635                 // compute ground-truth kinematics that should be generated at provided
31636                 // position, velocity and orientation
31637                 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
31638                         .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
31639                                 ecefFrame);
31640 
31641                 // apply known calibration parameters to distort ground-truth and generate a
31642                 // measured kinematics sample
31643                 final BodyKinematics measuredKinematics = BodyKinematicsGenerator
31644                         .generate(TIME_INTERVAL_SECONDS, trueKinematics, errors, random);
31645 
31646                 final StandardDeviationBodyKinematics measurement =
31647                         new StandardDeviationBodyKinematics(measuredKinematics,
31648                                 specificForceStandardDeviation,
31649                                 angularRateStandardDeviation);
31650                 measurements.add(measurement);
31651             }
31652 
31653             // When we have the minimum number of measurements, we need to provide
31654             // an initial solution close to the true solution
31655             final KnownPositionAccelerometerCalibrator calibrator =
31656                     new KnownPositionAccelerometerCalibrator(nedPosition,
31657                             measurements, false, ba, ma, this);
31658 
31659             // estimate
31660             reset();
31661             assertTrue(calibrator.isReady());
31662             assertFalse(calibrator.isRunning());
31663             assertEquals(mCalibrateStart, 0);
31664             assertEquals(mCalibrateEnd, 0);
31665 
31666             try {
31667                 calibrator.calibrate();
31668             } catch (final CalibrationException e) {
31669                 continue;
31670             }
31671 
31672             // check
31673             assertTrue(calibrator.isReady());
31674             assertFalse(calibrator.isRunning());
31675             assertEquals(mCalibrateStart, 1);
31676             assertEquals(mCalibrateEnd, 1);
31677 
31678             final Matrix estimatedBa = calibrator.getEstimatedBiasesAsMatrix();
31679             final Matrix estimatedMa = calibrator.getEstimatedMa();
31680 
31681             if (!ba.equals(estimatedBa, LARGE_ABSOLUTE_ERROR)) {
31682                 continue;
31683             }
31684             if (!ma.equals(estimatedMa, ABSOLUTE_ERROR)) {
31685                 continue;
31686             }
31687 
31688             assertTrue(ba.equals(estimatedBa, LARGE_ABSOLUTE_ERROR));
31689             assertTrue(ma.equals(estimatedMa, ABSOLUTE_ERROR));
31690 
31691             numValid++;
31692 
31693             break;
31694         }
31695 
31696         assertTrue(numValid > 0);
31697     }
31698 
31699     @Test
31700     public void testCalibrateForGeneralCaseWithLargeNumberOfMeasurementsAndNoise()
31701             throws WrongSizeException, InvalidSourceAndDestinationFrameTypeException,
31702             LockedException, NotReadyException {
31703 
31704         int numValid = 0;
31705         for (int t = 0; t < TIMES; t++) {
31706             final Matrix ba = generateBa();
31707             final Matrix bg = generateBg();
31708             final Matrix ma = generateMaGeneral();
31709             final Matrix mg = generateMg();
31710             final Matrix gg = generateGg();
31711             final double accelNoiseRootPSD = getAccelNoiseRootPSD();
31712             final double gyroNoiseRootPSD = getGyroNoiseRootPSD();
31713             final double accelQuantLevel = 0.0;
31714             final double gyroQuantLevel = 0.0;
31715 
31716             final IMUErrors errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD,
31717                     gyroNoiseRootPSD, accelQuantLevel, gyroQuantLevel);
31718 
31719             final Random random = new Random();
31720             final UniformRandomizer randomizer = new UniformRandomizer(random);
31721             final double latitude = Math.toRadians(
31722                     randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
31723             final double longitude = Math.toRadians(
31724                     randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
31725             final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
31726             final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
31727 
31728             final double sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
31729             final double specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
31730             final double angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
31731 
31732             final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();
31733             for (int i = 0; i < LARGE_MEASUREMENT_NUMBER; i++) {
31734                 final double roll = Math.toRadians(
31735                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31736                 final double pitch = Math.toRadians(
31737                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31738                 final double yaw = Math.toRadians(
31739                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31740                 final CoordinateTransformation nedC = new CoordinateTransformation(
31741                         roll, pitch, yaw, FrameType.BODY_FRAME,
31742                         FrameType.LOCAL_NAVIGATION_FRAME);
31743 
31744                 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
31745                 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
31746                         .convertNEDtoECEFAndReturnNew(nedFrame);
31747 
31748                 // compute ground-truth kinematics that should be generated at provided
31749                 // position, velocity and orientation
31750                 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
31751                         .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
31752                                 ecefFrame);
31753 
31754                 // apply known calibration parameters to distort ground-truth and generate a
31755                 // measured kinematics sample
31756                 final BodyKinematics measuredKinematics = BodyKinematicsGenerator
31757                         .generate(TIME_INTERVAL_SECONDS, trueKinematics, errors, random);
31758 
31759                 final StandardDeviationBodyKinematics measurement =
31760                         new StandardDeviationBodyKinematics(measuredKinematics,
31761                                 specificForceStandardDeviation,
31762                                 angularRateStandardDeviation);
31763                 measurements.add(measurement);
31764             }
31765 
31766             // When we have a large number of measurements, we do not need to provide
31767             // an initial solution as it will probably converge to true true solution
31768             final KnownPositionAccelerometerCalibrator calibrator =
31769                     new KnownPositionAccelerometerCalibrator(nedPosition,
31770                             measurements, false, this);
31771 
31772             // estimate
31773             reset();
31774             assertTrue(calibrator.isReady());
31775             assertFalse(calibrator.isRunning());
31776             assertEquals(mCalibrateStart, 0);
31777             assertEquals(mCalibrateEnd, 0);
31778 
31779             try {
31780                 calibrator.calibrate();
31781             } catch (final CalibrationException e) {
31782                 continue;
31783             }
31784 
31785             // check
31786             assertTrue(calibrator.isReady());
31787             assertFalse(calibrator.isRunning());
31788             assertEquals(mCalibrateStart, 1);
31789             assertEquals(mCalibrateEnd, 1);
31790 
31791             final Matrix estimatedBa = calibrator.getEstimatedBiasesAsMatrix();
31792             final Matrix estimatedMa = calibrator.getEstimatedMa();
31793 
31794             if (!ba.equals(estimatedBa, VERY_LARGE_ABSOLUTE_ERROR)) {
31795                 continue;
31796             }
31797             if (!ma.equals(estimatedMa, 6.0 * LARGE_ABSOLUTE_ERROR)) {
31798                 continue;
31799             }
31800 
31801             assertTrue(ba.equals(estimatedBa, VERY_LARGE_ABSOLUTE_ERROR));
31802             assertTrue(ma.equals(estimatedMa, 6.0 * LARGE_ABSOLUTE_ERROR));
31803 
31804             numValid++;
31805 
31806             break;
31807         }
31808 
31809         assertTrue(numValid > 0);
31810     }
31811 
31812     @Test
31813     public void testCalibrateForCommonAxisCaseWithMinimumMeasuresAndNoNoise()
31814             throws WrongSizeException, InvalidSourceAndDestinationFrameTypeException,
31815             LockedException, NotReadyException {
31816 
31817         int numValid = 0;
31818         for (int t = 0; t < TIMES; t++) {
31819             final Matrix ba = generateBa();
31820             final Matrix bg = generateBg();
31821             final Matrix ma = generateMaCommonAxis();
31822             final Matrix mg = generateMg();
31823             final Matrix gg = generateGg();
31824             // when using minimum number of measurements we must not add any noise so that
31825             // a solution is found, when adding more measurements, certain noise can be added
31826             final double accelNoiseRootPSD = 0.0;
31827             final double gyroNoiseRootPSD = 0.0;
31828             final double accelQuantLevel = 0.0;
31829             final double gyroQuantLevel = 0.0;
31830 
31831             final IMUErrors errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD,
31832                     gyroNoiseRootPSD, accelQuantLevel, gyroQuantLevel);
31833 
31834             final Random random = new Random();
31835             final UniformRandomizer randomizer = new UniformRandomizer(random);
31836             final double latitude = Math.toRadians(
31837                     randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
31838             final double longitude = Math.toRadians(
31839                     randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
31840             final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
31841             final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
31842 
31843             final double sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
31844             final double specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
31845             final double angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
31846 
31847             final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();
31848             for (int i = 0; i < KnownPositionAccelerometerCalibrator.MINIMUM_MEASUREMENTS_COMON_Z_AXIS; i++) {
31849                 final double roll = Math.toRadians(
31850                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31851                 final double pitch = Math.toRadians(
31852                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31853                 final double yaw = Math.toRadians(
31854                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31855                 final CoordinateTransformation nedC = new CoordinateTransformation(
31856                         roll, pitch, yaw, FrameType.BODY_FRAME,
31857                         FrameType.LOCAL_NAVIGATION_FRAME);
31858 
31859                 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
31860                 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
31861                         .convertNEDtoECEFAndReturnNew(nedFrame);
31862 
31863                 // compute ground-truth kinematics that should be generated at provided
31864                 // position, velocity and orientation
31865                 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
31866                         .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
31867                                 ecefFrame);
31868 
31869                 // apply known calibration parameters to distort ground-truth and generate a
31870                 // measured kinematics sample
31871                 final BodyKinematics measuredKinematics = BodyKinematicsGenerator
31872                         .generate(TIME_INTERVAL_SECONDS, trueKinematics, errors, random);
31873 
31874                 final StandardDeviationBodyKinematics measurement =
31875                         new StandardDeviationBodyKinematics(measuredKinematics,
31876                                 specificForceStandardDeviation,
31877                                 angularRateStandardDeviation);
31878                 measurements.add(measurement);
31879             }
31880 
31881             // When we have the minimum number of measurements, we need to provide
31882             // an initial solution close to the true solution
31883             final KnownPositionAccelerometerCalibrator calibrator =
31884                     new KnownPositionAccelerometerCalibrator(nedPosition,
31885                             measurements, true, ba, ma, this);
31886 
31887             // estimate
31888             reset();
31889             assertTrue(calibrator.isReady());
31890             assertFalse(calibrator.isRunning());
31891             assertEquals(mCalibrateStart, 0);
31892             assertEquals(mCalibrateEnd, 0);
31893 
31894             try {
31895                 calibrator.calibrate();
31896             } catch (final CalibrationException e) {
31897                 continue;
31898             }
31899 
31900             // check
31901             assertTrue(calibrator.isReady());
31902             assertFalse(calibrator.isRunning());
31903             assertEquals(mCalibrateStart, 1);
31904             assertEquals(mCalibrateEnd, 1);
31905 
31906             final Matrix estimatedBa = calibrator.getEstimatedBiasesAsMatrix();
31907             final Matrix estimatedMa = calibrator.getEstimatedMa();
31908 
31909             if (!ba.equals(estimatedBa, LARGE_ABSOLUTE_ERROR)) {
31910                 continue;
31911             }
31912             if (!ma.equals(estimatedMa, ABSOLUTE_ERROR)) {
31913                 continue;
31914             }
31915 
31916             assertTrue(ba.equals(estimatedBa, LARGE_ABSOLUTE_ERROR));
31917             assertTrue(ma.equals(estimatedMa, ABSOLUTE_ERROR));
31918 
31919             numValid++;
31920 
31921             break;
31922         }
31923 
31924         assertTrue(numValid > 0);
31925     }
31926 
31927     @Test
31928     public void testCalibrateForCommonAxisCaseWithLargeNumberOfMeasurementsAndNoise()
31929             throws WrongSizeException, InvalidSourceAndDestinationFrameTypeException,
31930             LockedException, NotReadyException {
31931 
31932         int numValid = 0;
31933         for (int t = 0; t < TIMES; t++) {
31934             final Matrix ba = generateBa();
31935             final Matrix bg = generateBg();
31936             final Matrix ma = generateMaCommonAxis();
31937             final Matrix mg = generateMg();
31938             final Matrix gg = generateGg();
31939             final double accelNoiseRootPSD = getAccelNoiseRootPSD();
31940             final double gyroNoiseRootPSD = getGyroNoiseRootPSD();
31941             final double accelQuantLevel = 0.0;
31942             final double gyroQuantLevel = 0.0;
31943 
31944             final IMUErrors errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD,
31945                     gyroNoiseRootPSD, accelQuantLevel, gyroQuantLevel);
31946 
31947             final Random random = new Random();
31948             final UniformRandomizer randomizer = new UniformRandomizer(random);
31949             final double latitude = Math.toRadians(
31950                     randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
31951             final double longitude = Math.toRadians(
31952                     randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
31953             final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
31954             final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
31955 
31956             final double sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
31957             final double specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
31958             final double angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
31959 
31960             final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();
31961             for (int i = 0; i < LARGE_MEASUREMENT_NUMBER; i++) {
31962                 final double roll = Math.toRadians(
31963                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31964                 final double pitch = Math.toRadians(
31965                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31966                 final double yaw = Math.toRadians(
31967                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31968                 final CoordinateTransformation nedC = new CoordinateTransformation(
31969                         roll, pitch, yaw, FrameType.BODY_FRAME,
31970                         FrameType.LOCAL_NAVIGATION_FRAME);
31971 
31972                 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
31973                 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
31974                         .convertNEDtoECEFAndReturnNew(nedFrame);
31975 
31976                 // compute ground-truth kinematics that should be generated at provided
31977                 // position, velocity and orientation
31978                 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
31979                         .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
31980                                 ecefFrame);
31981 
31982                 // apply known calibration parameters to distort ground-truth and generate a
31983                 // measured kinematics sample
31984                 final BodyKinematics measuredKinematics = BodyKinematicsGenerator
31985                         .generate(TIME_INTERVAL_SECONDS, trueKinematics, errors, random);
31986 
31987                 final StandardDeviationBodyKinematics measurement =
31988                         new StandardDeviationBodyKinematics(measuredKinematics,
31989                                 specificForceStandardDeviation,
31990                                 angularRateStandardDeviation);
31991                 measurements.add(measurement);
31992             }
31993 
31994             // When we have a large number of measurements, we do not need to provide
31995             // an initial solution as it will probably converge to true true solution
31996             final KnownPositionAccelerometerCalibrator calibrator =
31997                     new KnownPositionAccelerometerCalibrator(nedPosition,
31998                             measurements, true, this);
31999 
32000             // estimate
32001             reset();
32002             assertTrue(calibrator.isReady());
32003             assertFalse(calibrator.isRunning());
32004             assertEquals(mCalibrateStart, 0);
32005             assertEquals(mCalibrateEnd, 0);
32006 
32007             try {
32008                 calibrator.calibrate();
32009             } catch (final CalibrationException e) {
32010                 continue;
32011             }
32012 
32013             // check
32014             assertTrue(calibrator.isReady());
32015             assertFalse(calibrator.isRunning());
32016             assertEquals(mCalibrateStart, 1);
32017             assertEquals(mCalibrateEnd, 1);
32018 
32019             final Matrix estimatedBa = calibrator.getEstimatedBiasesAsMatrix();
32020             final Matrix estimatedMa = calibrator.getEstimatedMa();
32021 
32022             if (!ba.equals(estimatedBa, VERY_LARGE_ABSOLUTE_ERROR)) {
32023                 continue;
32024             }
32025             if (!ma.equals(estimatedMa, LARGE_ABSOLUTE_ERROR)) {
32026                 continue;
32027             }
32028 
32029             assertTrue(ba.equals(estimatedBa, VERY_LARGE_ABSOLUTE_ERROR));
32030             assertTrue(ma.equals(estimatedMa, LARGE_ABSOLUTE_ERROR));
32031 
32032             numValid++;
32033 
32034             break;
32035         }
32036 
32037         assertTrue(numValid > 0);
32038     }
32039 
32040     @Test
32041     public void testNorms() throws InvalidSourceAndDestinationFrameTypeException {
32042 
32043         // This test checks that the norm of true specific force and true angular
32044         // rates are equal to the gravity value (approximately 9.81 m/s^2), and the
32045         // Earth rotation rate respectively for any Earth location.
32046         // This property is the principle used for KnownPositionAccelerometerCalibrator
32047         // to be able to calibrate the accelerometer by taking kinemtics measurements
32048         // with unknown orientation, but known Earth location.
32049 
32050         final Random random = new Random();
32051         final UniformRandomizer randomizer = new UniformRandomizer(random);
32052         final double latitude = Math.toRadians(
32053                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
32054         final double longitude = Math.toRadians(
32055                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
32056         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
32057         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
32058 
32059         final double roll = Math.toRadians(
32060                 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
32061         final double pitch = Math.toRadians(
32062                 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
32063         final double yaw = Math.toRadians(
32064                 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
32065         final CoordinateTransformation nedC = new CoordinateTransformation(
32066                 roll, pitch, yaw, FrameType.BODY_FRAME,
32067                 FrameType.LOCAL_NAVIGATION_FRAME);
32068 
32069         final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
32070         final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
32071                 .convertNEDtoECEFAndReturnNew(nedFrame);
32072 
32073         final BodyKinematics trueKinematics = ECEFKinematicsEstimator
32074                 .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
32075                         ecefFrame);
32076 
32077         final double fNorm = trueKinematics.getSpecificForceNorm();
32078         final double wNorm = trueKinematics.getAngularRateNorm();
32079 
32080         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(ecefFrame);
32081         final double g = gravity.getNorm();
32082 
32083         assertEquals(g, fNorm, ABSOLUTE_ERROR);
32084         assertEquals(Constants.EARTH_ROTATION_RATE, wNorm, ABSOLUTE_ERROR);
32085     }
32086 
32087     @Override
32088     public void onCalibrateStart(
32089             final KnownPositionAccelerometerCalibrator calibrator) {
32090         checkLocked(calibrator);
32091         mCalibrateStart++;
32092     }
32093 
32094     @Override
32095     public void onCalibrateEnd(
32096             final KnownPositionAccelerometerCalibrator calibrator) {
32097         checkLocked(calibrator);
32098         mCalibrateEnd++;
32099     }
32100 
32101     private void reset() {
32102         mCalibrateStart = 0;
32103         mCalibrateEnd = 0;
32104     }
32105 
32106     private void checkLocked(final KnownPositionAccelerometerCalibrator calibrator) {
32107         assertTrue(calibrator.isRunning());
32108         try {
32109             calibrator.setInitialBiasX(0.0);
32110             fail("LockedException expected but not thrown");
32111         } catch (final LockedException ignore) {
32112         }
32113         try {
32114             calibrator.setInitialBiasY(0.0);
32115             fail("LockedException expected but not thrown");
32116         } catch (final LockedException ignore) {
32117         }
32118         try {
32119             calibrator.setInitialBiasZ(0.0);
32120             fail("LockedException expected but not thrown");
32121         } catch (final LockedException ignore) {
32122         }
32123         try {
32124             calibrator.setInitialBiasX(null);
32125             fail("LockedException expected but not thrown");
32126         } catch (final LockedException ignore) {
32127         }
32128         try {
32129             calibrator.setInitialBiasY(null);
32130             fail("LockedException expected but not thrown");
32131         } catch (final LockedException ignore) {
32132         }
32133         try {
32134             calibrator.setInitialBiasZ(null);
32135             fail("LockedException expected but not thrown");
32136         } catch (final LockedException ignore) {
32137         }
32138         try {
32139             calibrator.setInitialBias(0.0, 0.0, 0.0);
32140             fail("LockedException expected but not thrown");
32141         } catch (final LockedException ignore) {
32142         }
32143         try {
32144             calibrator.setInitialBias(null, null, null);
32145             fail("LockedException expected but not thrown");
32146         } catch (final LockedException ignore) {
32147         }
32148         try {
32149             calibrator.setInitialSx(0.0);
32150             fail("LockedException expected but not thrown");
32151         } catch (final LockedException ignore) {
32152         }
32153         try {
32154             calibrator.setInitialSy(0.0);
32155             fail("LockedException expected but not thrown");
32156         } catch (final LockedException ignore) {
32157         }
32158         try {
32159             calibrator.setInitialSz(0.0);
32160             fail("LockedException expected but not thrown");
32161         } catch (final LockedException ignore) {
32162         }
32163         try {
32164             calibrator.setInitialMxy(0.0);
32165             fail("LockedException expected but not thrown");
32166         } catch (final LockedException ignore) {
32167         }
32168         try {
32169             calibrator.setInitialMxz(0.0);
32170             fail("LockedException expected but not thrown");
32171         } catch (final LockedException ignore) {
32172         }
32173         try {
32174             calibrator.setInitialMyx(0.0);
32175             fail("LockedException expected but not thrown");
32176         } catch (final LockedException ignore) {
32177         }
32178         try {
32179             calibrator.setInitialMyz(0.0);
32180             fail("LockedException expected but not thrown");
32181         } catch (final LockedException ignore) {
32182         }
32183         try {
32184             calibrator.setInitialMzx(0.0);
32185             fail("LockedException expected but not thrown");
32186         } catch (final LockedException ignore) {
32187         }
32188         try {
32189             calibrator.setInitialMzy(0.0);
32190             fail("LockedException expected but not thrown");
32191         } catch (final LockedException ignore) {
32192         }
32193         try {
32194             calibrator.setInitialScalingFactors(0.0, 0.0, 0.0);
32195             fail("LockedException expected but not thrown");
32196         } catch (final LockedException ignore) {
32197         }
32198         try {
32199             calibrator.setInitialCrossCouplingErrors(
32200                     0.0, 0.0, 0.0,
32201                     0.0, 0.0, 0.0);
32202             fail("LockedException expected but not thrown");
32203         } catch (final LockedException ignore) {
32204         }
32205         try {
32206             calibrator.setInitialScalingFactorsAndCrossCouplingErrors(
32207                     0.0, 0.0, 0.0,
32208                     0.0, 0.0, 0.0,
32209                     0.0, 0.0, 0.0);
32210             fail("LockedException expected but not thrown");
32211         } catch (final LockedException ignore) {
32212         }
32213         try {
32214             calibrator.setInitialBias((double[]) null);
32215             fail("LockedException expected but not thrown");
32216         } catch (final LockedException ignore) {
32217         }
32218         try {
32219             calibrator.setInitialBias((Matrix) null);
32220             fail("LockedException expected but not thrown");
32221         } catch (final LockedException ignore) {
32222         }
32223         try {
32224             calibrator.setInitialMa(null);
32225             fail("LockedException expected but not thrown");
32226         } catch (final LockedException ignore) {
32227         }
32228         try {
32229             calibrator.setPosition((ECEFPosition) null);
32230             fail("LockedException expected but not thrown");
32231         } catch (final LockedException ignore) {
32232         }
32233         try {
32234             calibrator.setPosition((NEDPosition) null);
32235             fail("LockedException expected but not thrown");
32236         } catch (final LockedException ignore) {
32237         }
32238         try {
32239             calibrator.setMeasurements(null);
32240             fail("LockedException expected but not thrown");
32241         } catch (final LockedException ignore) {
32242         }
32243         try {
32244             calibrator.setCommonAxisUsed(true);
32245             fail("LockedException expected but not thrown");
32246         } catch (final LockedException ignore) {
32247         }
32248         try {
32249             calibrator.setListener(this);
32250             fail("LockedException expected but not thrown");
32251         } catch (final LockedException ignore) {
32252         }
32253         try {
32254             calibrator.calibrate();
32255             fail("LockedException expected but not thrown");
32256         } catch (final LockedException ignore) {
32257         } catch (final Exception e) {
32258             fail("LockedException expected but not thrown");
32259         }
32260     }
32261 
32262     private Matrix generateBa() {
32263         return Matrix.newFromArray(new double[]{
32264                 900 * MICRO_G_TO_METERS_PER_SECOND_SQUARED,
32265                 -1300 * MICRO_G_TO_METERS_PER_SECOND_SQUARED,
32266                 800 * MICRO_G_TO_METERS_PER_SECOND_SQUARED});
32267     }
32268 
32269     private Matrix generateBg() {
32270         return Matrix.newFromArray(new double[]{
32271                 -9 * DEG_TO_RAD / 3600.0,
32272                 13 * DEG_TO_RAD / 3600.0,
32273                 -8 * DEG_TO_RAD / 3600.0});
32274     }
32275 
32276     private Matrix generateMaGeneral() throws WrongSizeException {
32277         final Matrix result = new Matrix(3, 3);
32278         result.fromArray(new double[]{
32279                 500e-6, -300e-6, 200e-6,
32280                 -150e-6, -600e-6, 250e-6,
32281                 -250e-6, 100e-6, 450e-6
32282         }, false);
32283 
32284         return result;
32285     }
32286 
32287     private Matrix generateMaCommonAxis() throws WrongSizeException {
32288         final Matrix result = new Matrix(3, 3);
32289         result.fromArray(new double[]{
32290                 500e-6, -300e-6, 200e-6,
32291                 0.0, -600e-6, 250e-6,
32292                 0.0, 0.0, 450e-6
32293         }, false);
32294 
32295         return result;
32296     }
32297 
32298     private Matrix generateMg() throws WrongSizeException {
32299         final Matrix result = new Matrix(3, 3);
32300         result.fromArray(new double[]{
32301                 400e-6, -300e-6, 250e-6,
32302                 0.0, -300e-6, -150e-6,
32303                 0.0, 0.0, -350e-6
32304         }, false);
32305 
32306         return result;
32307     }
32308 
32309     private Matrix generateGg() throws WrongSizeException {
32310         final Matrix result = new Matrix(3, 3);
32311         final double tmp = DEG_TO_RAD / (3600 * 9.80665);
32312         result.fromArray(new double[]{
32313                 0.9 * tmp, -1.1 * tmp, -0.6 * tmp,
32314                 -0.5 * tmp, 1.9 * tmp, -1.6 * tmp,
32315                 0.3 * tmp, 1.1 * tmp, -1.3 * tmp
32316         }, false);
32317 
32318         return result;
32319     }
32320 
32321     private double getAccelNoiseRootPSD() {
32322         return 100.0 * MICRO_G_TO_METERS_PER_SECOND_SQUARED;
32323     }
32324 
32325     private double getGyroNoiseRootPSD() {
32326         return 0.01 * DEG_TO_RAD / 60.0;
32327     }
32328 }